From 15225da78339ac094b63672575f64e4dc1410808 Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Thu, 1 Aug 2024 12:08:06 -0600 Subject: [PATCH 1/8] Initial swap tow chapter 14 dynamics. This publishes and calculates forces, but they are all zero for the moment. --- .../multirotor_forces_and_moments.hpp | 44 ++- rosflight_sim/params/multirotor_dynamics.yaml | 35 ++- .../src/multirotor_forces_and_moments.cpp | 256 +++++++++--------- 3 files changed, 174 insertions(+), 161 deletions(-) diff --git a/rosflight_sim/include/rosflight_sim/multirotor_forces_and_moments.hpp b/rosflight_sim/include/rosflight_sim/multirotor_forces_and_moments.hpp index 7e6b2fda..1adab04a 100644 --- a/rosflight_sim/include/rosflight_sim/multirotor_forces_and_moments.hpp +++ b/rosflight_sim/include/rosflight_sim/multirotor_forces_and_moments.hpp @@ -3,6 +3,7 @@ * * Copyright (c) 2017 Daniel Koch, James Jackson and Gary Ellingson, BYU MAGICC Lab. * Copyright (c) 2023 Brandon Sutherland, AeroVironment Inc. + * Copyright (c) 2024 Ian Reid, BYU MAGICC Lab. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -35,6 +36,7 @@ #ifndef ROSFLIGHT_SIM_MULTIROTOR_FORCES_AND_MOMENTS_H #define ROSFLIGHT_SIM_MULTIROTOR_FORCES_AND_MOMENTS_H +#include #include #include @@ -56,24 +58,40 @@ class Multirotor : public MAVForcesAndMoments private: rclcpp::Node::SharedPtr node_; Eigen::Vector3d wind_; - - double prev_time_; - - struct Rotor + + struct Prop { - double max; - std::vector F_poly; - std::vector T_poly; - double tau_up; // time constants for response - double tau_down; + // Prop thrust constant coeffs. + double CT_0; + double CT_1; + double CT_2; + + // Prop torque constant coeffs. + double CQ_0; + double CQ_1; + double CQ_2; + + double diam; }; struct Motor { - Rotor rotor; - Eigen::Vector3d position; - Eigen::Vector3d normal; - int direction; // 1 for CW -1 for CCW + double dist; + // The motor's radial angle from the body north unit vector in radians, + // for example: quad copter x frame this is 45,135,225,315, except in radians. + double radial_angle; + // The direction that the motor spins, (-1 is clockwise and 1 is counter clocwise, looking from above vehicle). + int64_t direction; + // The current command for the motor, from 0 to 1 + double command; + // Resistance of motor. + double R; + // Torque constant of motor. FIXME: is this the right name? + double KQ; + // No load current draw of motor. FIXME: is this the right name? + double I_0; + // Prop data + Prop prop; }; int num_rotors_; diff --git a/rosflight_sim/params/multirotor_dynamics.yaml b/rosflight_sim/params/multirotor_dynamics.yaml index 838efcc4..b83c29a3 100644 --- a/rosflight_sim/params/multirotor_dynamics.yaml +++ b/rosflight_sim/params/multirotor_dynamics.yaml @@ -4,9 +4,7 @@ /multirotor/rosflight_sil: ros__parameters: # Common Global Physical Parameters - linear_mu: 0.05 - angular_mu: 0.0005 - ground_effect: [ -55.3516, 181.8265, -203.9874, 85.3735, -7.6619 ] + rho: 1.225 # Dynamics num_rotors: 4 @@ -14,15 +12,26 @@ -0.1907, 0.205, -0.0762, -0.1907, -0.205, -0.0762, 0.1926, -0.230, -0.0762 ] + + rotor_dists: [.25, .25, .25, .25] + rotor_radial_angles: [45., 135., 225., 315.] + rotor_rotation_directions: [ -1, 1, -1, 1 ] + + # Prop Model + # Thrust + CT_0: 0.1196850 + CT_1: 0.0399995 + CT_2: -0.400139 - rotor_vector_normal: [ -0.02674078, 0.0223925, -0.99939157, - 0.02553726, 0.02375588, -0.99939157, - 0.02553726, -0.02375588, -0.99939157, - -0.02674078, -0.0223925, -0.99939157 ] + # Torque + CQ_0: 0.0097976 + CQ_1: -0.012271 + CQ_2: -0.014743 - rotor_rotation_directions: [ -1, 1, -1, 1 ] - rotor_max_thrust: 14.961 - rotor_F: [ 1.5e-5, -0.024451, 9.00225 ] - rotor_T: [ 2.22e-7,-3.51e-4, 0.12531 ] - rotor_tau_up: 0.2164 - rotor_tau_down: 0.1644 + # Diameter (m) + D_prop: 0.381 + + V_max : 24.0 # voltage for 6s battery at 4 volts per cell + R_motor : 0.042 # ohms + I_0 : 1.5 # no-load (zero torque) current (A) + KQ : 0.01705 #((1 /(KV_rpm_per_volt)) *60) / (2 *M_PI) Back-emf constant, KV in V-s/rad, Motor torque constant, KQ in N-m/A diff --git a/rosflight_sim/src/multirotor_forces_and_moments.cpp b/rosflight_sim/src/multirotor_forces_and_moments.cpp index ee400729..08e83ddf 100644 --- a/rosflight_sim/src/multirotor_forces_and_moments.cpp +++ b/rosflight_sim/src/multirotor_forces_and_moments.cpp @@ -32,6 +32,9 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include +#include +#include #include namespace rosflight_sim @@ -44,98 +47,43 @@ Multirotor::Multirotor(rclcpp::Node::SharedPtr node) { declare_multirotor_params(); - if (!node_->get_parameter("linear_mu", linear_mu_)) { - RCLCPP_ERROR(node_->get_logger(), "Param 'linear_mu' not defined"); - } - if (!node_->get_parameter("angular_mu", angular_mu_)) { - RCLCPP_ERROR(node_->get_logger(), "Param 'angular_mu' not defined"); - } - if (!node_->get_parameter("ground_effect", ground_effect_)) { - RCLCPP_ERROR(node_->get_logger(), "Param 'ground_effect' not defined"); - } - - if (!node_->get_parameter("num_rotors", num_rotors_)) { - RCLCPP_ERROR(node_->get_logger(), "Param 'num_rotors' not defined"); - } - std::vector rotor_positions(3 * num_rotors_); std::vector rotor_vector_normal(3 * num_rotors_); std::vector rotor_rotation_directions(num_rotors_); - Rotor rotor; - if (!node_->get_parameter("rotor_positions", rotor_positions)) { - RCLCPP_ERROR(node_->get_logger(), "Param 'rotor_positions' not defined"); - } - if (!node_->get_parameter("rotor_vector_normal", rotor_vector_normal)) { - RCLCPP_ERROR(node_->get_logger(), "Param 'rotor_vector_normal' not defined"); - } - - if (!node_->get_parameter("rotor_rotation_directions", rotor_rotation_directions)) { - RCLCPP_ERROR(node_->get_logger(), "Param 'rotor_rotation_directions' not defined"); - } - if (!node_->get_parameter("rotor_max_thrust", rotor.max)) { - RCLCPP_ERROR(node_->get_logger(), "Param 'rotor_max_thrust' not defined"); - } - if (!node_->get_parameter("rotor_F", rotor.F_poly)) { - RCLCPP_ERROR(node_->get_logger(), "Param 'rotor_F' not defined"); - } - if (!node_->get_parameter("rotor_T", rotor.T_poly)) { - RCLCPP_ERROR(node_->get_logger(), "Param 'rotor_T' not defined"); - } - if (!node_->get_parameter("rotor_tau_up", rotor.tau_up)) { - RCLCPP_ERROR(node_->get_logger(), "Param 'rotor_tau_up' not defined"); - } - if (!node_->get_parameter("rotor_tau_down", rotor.tau_down)) { - RCLCPP_ERROR(node_->get_logger(), "Param 'rotor_tau_down' not defined"); - } - /* Load Rotor Configuration */ motors_.resize(num_rotors_); + + Prop prop; - force_allocation_matrix_.resize(4, num_rotors_); - torque_allocation_matrix_.resize(4, num_rotors_); - for (int i = 0; i < num_rotors_; i++) { - motors_[i].rotor = rotor; - motors_[i].position.resize(3); - motors_[i].normal.resize(3); - for (int j = 0; j < 3; j++) { - motors_[i].position(j) = rotor_positions[3 * i + j]; - motors_[i].normal(j) = rotor_vector_normal[3 * i + j]; - } - motors_[i].normal.normalize(); - motors_[i].direction = (int) rotor_rotation_directions[i]; - - Eigen::Vector3d moment_from_thrust = motors_[i].position.cross(motors_[i].normal); - Eigen::Vector3d moment_from_torque = motors_[i].direction * motors_[i].normal; - - // build allocation_matrices - force_allocation_matrix_(0, i) = moment_from_thrust(0); // l - force_allocation_matrix_(1, i) = moment_from_thrust(1); // m - force_allocation_matrix_(2, i) = moment_from_thrust(2); // n - force_allocation_matrix_(3, i) = motors_[i].normal(2); // F - - torque_allocation_matrix_(0, i) = moment_from_torque(0); // l - torque_allocation_matrix_(1, i) = moment_from_torque(1); // m - torque_allocation_matrix_(2, i) = moment_from_torque(2); // n - torque_allocation_matrix_(3, i) = 0.0; // F - } - - RCLCPP_INFO_STREAM(node_->get_logger(), - "allocation matrices:\nFORCE \n" - << force_allocation_matrix_ << "\nTORQUE\n" - << torque_allocation_matrix_ << "\n"); + prop.CT_0 = node_->get_parameter("CT_0").as_double(); + prop.CT_1 = node_->get_parameter("CT_1").as_double(); + prop.CT_2 = node_->get_parameter("CT_2").as_double(); + + prop.CQ_0 = node_->get_parameter("CQ_0").as_double(); + prop.CQ_1 = node_->get_parameter("CQ_1").as_double(); + prop.CQ_2 = node_->get_parameter("CQ_2").as_double(); - // Initialize size of dynamic force and torque matrices - desired_forces_.resize(num_rotors_); - desired_torques_.resize(num_rotors_); - actual_forces_.resize(num_rotors_); - actual_torques_.resize(num_rotors_); + prop.diam = node_->get_parameter("D_prop").as_double(); + + std::vector dists = node_->get_parameter("rotor_dists").as_double_array(); + std::vector angles = node_->get_parameter("rotor_radial_angles").as_double_array(); + std::vector rotation_dirs = node_->get_parameter("rotor_rotation_directions").as_integer_array(); + + Motor motor_characteristics; + motor_characteristics.prop = prop; + motor_characteristics.R = node_->get_parameter("R_motor").as_double(); + motor_characteristics.I_0 = node_->get_parameter("I_0").as_double(); + motor_characteristics.KQ = node_->get_parameter("KQ").as_double(); + for (int i = 0; i < num_rotors_; i++) { - desired_forces_(i) = 0.0; - desired_torques_(i) = 0.0; - actual_forces_(i) = 0.0; - actual_torques_(i) = 0.0; + Motor motor = motor_characteristics; + motor.dist = dists[i]; + motor.radial_angle = angles[i]; + motor.direction = rotation_dirs[i]; + motor.command = 0.0; + motors_[i] = motor; } wind_ = Eigen::Vector3d::Zero(); @@ -143,83 +91,121 @@ Multirotor::Multirotor(rclcpp::Node::SharedPtr node) forces_moments_pub_ = node_->create_publisher("/forces_and_moments", 1); - prev_time_ = -1; } Multirotor::~Multirotor() = default; void Multirotor::declare_multirotor_params() { - node_->declare_parameter("mass", rclcpp::PARAMETER_DOUBLE); - node_->declare_parameter("linear_mu", rclcpp::PARAMETER_DOUBLE); - node_->declare_parameter("angular_mu", rclcpp::PARAMETER_DOUBLE); - node_->declare_parameter("ground_effect", rclcpp::PARAMETER_DOUBLE_ARRAY); + // node_->declare_parameter("mass", rclcpp::PARAMETER_DOUBLE); node_->declare_parameter("num_rotors", rclcpp::PARAMETER_INTEGER); - node_->declare_parameter("rotor_positions", rclcpp::PARAMETER_DOUBLE_ARRAY); - node_->declare_parameter("rotor_vector_normal", rclcpp::PARAMETER_DOUBLE_ARRAY); + node_->declare_parameter("CT_0", rclcpp::PARAMETER_DOUBLE); + node_->declare_parameter("CT_1", rclcpp::PARAMETER_DOUBLE); + node_->declare_parameter("CT_2", rclcpp::PARAMETER_DOUBLE); + node_->declare_parameter("CQ_0", rclcpp::PARAMETER_DOUBLE); + node_->declare_parameter("CQ_1", rclcpp::PARAMETER_DOUBLE); + node_->declare_parameter("CQ_2", rclcpp::PARAMETER_DOUBLE); + + node_->declare_parameter("KQ", rclcpp::PARAMETER_DOUBLE); + node_->declare_parameter("V_max", rclcpp::PARAMETER_DOUBLE); + node_->declare_parameter("R_motor", rclcpp::PARAMETER_DOUBLE); + node_->declare_parameter("I_0", rclcpp::PARAMETER_DOUBLE); + node_->declare_parameter("rotor_dists", rclcpp::PARAMETER_DOUBLE_ARRAY); + node_->declare_parameter("rotor_radial_angles", rclcpp::PARAMETER_DOUBLE_ARRAY); node_->declare_parameter("rotor_rotation_directions", rclcpp::PARAMETER_INTEGER_ARRAY); - node_->declare_parameter("rotor_max_thrust", rclcpp::PARAMETER_DOUBLE); - node_->declare_parameter("rotor_F", rclcpp::PARAMETER_DOUBLE_ARRAY); - node_->declare_parameter("rotor_T", rclcpp::PARAMETER_DOUBLE_ARRAY); - node_->declare_parameter("rotor_tau_up", rclcpp::PARAMETER_DOUBLE); - node_->declare_parameter("rotor_tau_down", rclcpp::PARAMETER_DOUBLE); + + node_->declare_parameter("D_prop", rclcpp::PARAMETER_DOUBLE); + node_->declare_parameter("rho", rclcpp::PARAMETER_DOUBLE); // TODO: make sure these have default parameters. } Eigen::Matrix Multirotor::update_forces_and_torques(CurrentState x, const int act_cmds[]) { - if (prev_time_ < 0) { - prev_time_ = x.t; - return Eigen::Matrix::Zero(); - } + // The maximum voltage of the battery. + double V_max = node_->get_parameter("V_max").as_double(); - double dt = x.t - prev_time_; - double pd = x.pos[2]; + // double KQ = node_->get_parameter("KQ").as_double(); + // double R_motor = node_->get_parameter("R_motor").as_double(); + // double I_0 = node_->get_parameter("I_0").as_double(); + + double rho = node_->get_parameter("rho").as_double(); // Get airspeed vector for drag force calculation (rotate wind into body frame and add to inertial velocity) Eigen::Vector3d Va = x.vel + x.rot.inverse() * wind_; - // Calculate Forces - for (int i = 0; i < num_rotors_; i++) { - // First, figure out the desired force output from passing the signal into the quadratic approximation - double signal = act_cmds[i]; - desired_forces_(i, 0) = motors_[i].rotor.F_poly[0] * signal * signal - + motors_[i].rotor.F_poly[1] * signal + motors_[i].rotor.F_poly[2]; - desired_torques_(i, 0) = motors_[i].rotor.T_poly[0] * signal * signal - + motors_[i].rotor.T_poly[1] * signal + motors_[i].rotor.T_poly[2]; - - // Then, Calculate Actual force and torque for each rotor using first-order dynamics - double tau = (desired_forces_(i, 0) > actual_forces_(i, 0)) ? motors_[i].rotor.tau_up - : motors_[i].rotor.tau_down; - double alpha = dt / (tau + dt); - actual_forces_(i, 0) = - sat((1 - alpha) * actual_forces_(i) + alpha * desired_forces_(i), motors_[i].rotor.max, 0.0); - actual_torques_(i, 0) = sat((1 - alpha) * actual_torques_(i) + alpha * desired_torques_(i), - motors_[i].rotor.max, 0.0); - } - - // Use the allocation matrix to calculate the body-fixed force and torques - Eigen::Vector4d output_forces = force_allocation_matrix_ * actual_forces_; - Eigen::Vector4d output_torques = torque_allocation_matrix_ * actual_torques_; - Eigen::Vector4d output_forces_and_torques = output_forces + output_torques; + // TODO: Wrap this whole thing in a loop for each motor. + + // The motor's straight line distance from the center of mass. + double motor_dist; + // The motor's radial angle from the body north unit vector in radians, + // for example: quad copter x frame this is 45,135,225,315, except in radians. + double motor_radial_angle; + // The direction that the motor spins, (-1 is clockwise and 1 is counter clocwise, looking from above vehicle). + int motor_direction; + // The motor_command from 0 to 1. + double motor_command; + + double total_thrust = 0.0; + double total_roll_torque = 0.0; + double total_pitch_torque = 0.0; + double total_yaw_torque = 0.0; + + for (Motor motor : motors_) { + // Calculate prop_speed + // Use model with V_max + double v_in = V_max * motor.command; + + double a = (motor.prop.CQ_0 * (rho) * (pow((motor.prop.diam), 5.0)) / (pow((2 * M_PI), 2.0))); + double b = (motor.prop.CQ_1 * (rho) * (pow((motor.prop.diam), 4.0)) / (2 * M_PI)) * Va.norm() + + ((pow((motor.KQ), 2.0)) / (motor.R)); + double c = (motor.prop.CQ_2 * (rho) * (pow((motor.prop.diam), 3.0)) * (pow(Va.norm(), 2.0))) + - ((motor.KQ) / (motor.R)) * v_in + ((motor.KQ) * (motor.I_0)); + + double prop_speed = ((-b + sqrt((pow((b), 2.0)) - (4 * a * c))) / (2 * a)); + + // Calculate the advance ratio. + double advance_ratio = (2. * M_PI * Va.norm())/(prop_speed*motor.prop.diam); + + double CT = motor.prop.CT_2*pow(advance_ratio, 2) + motor.prop.CT_1*advance_ratio + motor.prop.CT_0; + + double motor_thrust = CT * (rho * pow(motor.prop.diam, 4))/(4*pow(M_PI,2)) * pow(prop_speed, 2); + + double motor_roll_torque = motor_thrust*motor_dist*sinf(motor_radial_angle); + double motor_pitch_torque = motor_thrust*motor_dist*cosf(motor_radial_angle); + + double CQ = motor.prop.CQ_2*pow(advance_ratio, 2) + motor.prop.CQ_1*advance_ratio + motor.prop.CQ_0; + + // The torque produced by the propeller spinning. + double prop_torque = motor_direction * CQ * (rho*pow(motor.prop.diam, 5))/(4*pow(M_PI,2)) * pow(prop_speed, 2); // TODO: Do you need this? + + double motor_yaw_torque = motor_thrust*motor_dist*cosf(motor_radial_angle); + + total_thrust += motor_thrust; + total_roll_torque += motor_roll_torque; + total_pitch_torque += motor_pitch_torque; + total_yaw_torque += motor_yaw_torque; + } + + // Rotate from body to inertial frame. + + Eigen::Vector3d body_forces; + body_forces << 0.0, 0.0, total_thrust; + + Eigen::Vector3d body_torques; + body_torques << total_roll_torque, total_pitch_torque, total_yaw_torque; - // Calculate Ground Effect - double z = -pd; - double ground_effect = - max(ground_effect_[0] * z * z * z * z + ground_effect_[1] * z * z * z - + ground_effect_[2] * z * z + ground_effect_[3] * z + ground_effect_[4], - 0); + Eigen::Vector3d inertial_forces = x.rot.inverse() * body_forces; + Eigen::Vector3d inertial_torques = x.rot.inverse() * body_torques; Eigen::Matrix forces; - // Apply other forces (drag) <- follows "Quadrotors and Accelerometers - State Estimation With an Improved Dynamic - // Model" By Rob Leishman et al. - forces.block<3, 1>(0, 0) = -linear_mu_ * Va; - forces.block<3, 1>(3, 0) = -angular_mu_ * x.omega + output_forces_and_torques.block<3, 1>(0, 0); + forces.block<3,1>(0,0) = inertial_forces; + forces.block<3,1>(3,0) = inertial_torques; + + // TODO: Apply drag. - // Apply ground effect and thrust - forces(2) += output_forces_and_torques(3) - ground_effect; + // TODO: Apply ground effect geometry_msgs::msg::TwistStamped msg; From 413db132526005bf82d80dcaf68caa38238b996d Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Thu, 1 Aug 2024 12:49:41 -0600 Subject: [PATCH 2/8] Got actuator commands and forces to activate. The multirotor flips and crashes to nans very quickly. --- .../src/multirotor_forces_and_moments.cpp | 37 +++++++++---------- 1 file changed, 17 insertions(+), 20 deletions(-) diff --git a/rosflight_sim/src/multirotor_forces_and_moments.cpp b/rosflight_sim/src/multirotor_forces_and_moments.cpp index 08e83ddf..f9464b57 100644 --- a/rosflight_sim/src/multirotor_forces_and_moments.cpp +++ b/rosflight_sim/src/multirotor_forces_and_moments.cpp @@ -47,9 +47,9 @@ Multirotor::Multirotor(rclcpp::Node::SharedPtr node) { declare_multirotor_params(); - std::vector rotor_positions(3 * num_rotors_); - std::vector rotor_vector_normal(3 * num_rotors_); - std::vector rotor_rotation_directions(num_rotors_); + + num_rotors_ = node_->get_parameter("num_rotors").as_int(); + /* Load Rotor Configuration */ motors_.resize(num_rotors_); @@ -135,30 +135,27 @@ Eigen::Matrix Multirotor::update_forces_and_torques(CurrentState x // Get airspeed vector for drag force calculation (rotate wind into body frame and add to inertial velocity) Eigen::Vector3d Va = x.vel + x.rot.inverse() * wind_; - // TODO: Wrap this whole thing in a loop for each motor. - - // The motor's straight line distance from the center of mass. - double motor_dist; - // The motor's radial angle from the body north unit vector in radians, - // for example: quad copter x frame this is 45,135,225,315, except in radians. - double motor_radial_angle; - // The direction that the motor spins, (-1 is clockwise and 1 is counter clocwise, looking from above vehicle). - int motor_direction; - // The motor_command from 0 to 1. - double motor_command; - double total_thrust = 0.0; double total_roll_torque = 0.0; double total_pitch_torque = 0.0; double total_yaw_torque = 0.0; + for (int i = 0; i < num_rotors_; i++) { + + double motor_cmd = (act_cmds[i] - 1000)/1000.0; + + std::cout << "motor_cmd: " << motor_cmd << "\n"; + + motors_[i].command = motor_cmd; + } + for (Motor motor : motors_) { // Calculate prop_speed // Use model with V_max double v_in = V_max * motor.command; double a = (motor.prop.CQ_0 * (rho) * (pow((motor.prop.diam), 5.0)) / (pow((2 * M_PI), 2.0))); - double b = (motor.prop.CQ_1 * (rho) * (pow((motor.prop.diam), 4.0)) / (2 * M_PI)) * Va.norm() + double b = (motor.prop.CQ_1 * (rho) * (pow((motor.prop.diam), 4.0)) / (2 * M_PI)) * Va.norm() // FIXME: The Va.norm() is actually just the portion parrallel to the prop's thrust. + ((pow((motor.KQ), 2.0)) / (motor.R)); double c = (motor.prop.CQ_2 * (rho) * (pow((motor.prop.diam), 3.0)) * (pow(Va.norm(), 2.0))) - ((motor.KQ) / (motor.R)) * v_in + ((motor.KQ) * (motor.I_0)); @@ -172,15 +169,15 @@ Eigen::Matrix Multirotor::update_forces_and_torques(CurrentState x double motor_thrust = CT * (rho * pow(motor.prop.diam, 4))/(4*pow(M_PI,2)) * pow(prop_speed, 2); - double motor_roll_torque = motor_thrust*motor_dist*sinf(motor_radial_angle); - double motor_pitch_torque = motor_thrust*motor_dist*cosf(motor_radial_angle); + double motor_roll_torque = motor_thrust*motor.dist*sinf(motor.radial_angle); + double motor_pitch_torque = motor_thrust*motor.dist*cosf(motor.radial_angle); double CQ = motor.prop.CQ_2*pow(advance_ratio, 2) + motor.prop.CQ_1*advance_ratio + motor.prop.CQ_0; // The torque produced by the propeller spinning. - double prop_torque = motor_direction * CQ * (rho*pow(motor.prop.diam, 5))/(4*pow(M_PI,2)) * pow(prop_speed, 2); // TODO: Do you need this? + double prop_torque = motor.direction * CQ * (rho*pow(motor.prop.diam, 5))/(4*pow(M_PI,2)) * pow(prop_speed, 2); // TODO: Do you need this? - double motor_yaw_torque = motor_thrust*motor_dist*cosf(motor_radial_angle); + double motor_yaw_torque = motor_thrust*motor.dist*cosf(motor.radial_angle); total_thrust += motor_thrust; total_roll_torque += motor_roll_torque; From 5d1400b6898922db572115173921aa4844383c76 Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Thu, 1 Aug 2024 15:00:18 -0600 Subject: [PATCH 3/8] Fixed forces and moments. Aircraft flies! The aircraft flies for a good chunk of time, but it randomly will acclerate to the ground way to fast. Also the issue was that the forces were being transformed to the inertial frame. But the forces should be given in NED body frame. --- .../src/multirotor_forces_and_moments.cpp | 31 ++++++++++--------- 1 file changed, 16 insertions(+), 15 deletions(-) diff --git a/rosflight_sim/src/multirotor_forces_and_moments.cpp b/rosflight_sim/src/multirotor_forces_and_moments.cpp index f9464b57..96cedfc2 100644 --- a/rosflight_sim/src/multirotor_forces_and_moments.cpp +++ b/rosflight_sim/src/multirotor_forces_and_moments.cpp @@ -32,6 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include #include #include #include @@ -80,7 +81,7 @@ Multirotor::Multirotor(rclcpp::Node::SharedPtr node) for (int i = 0; i < num_rotors_; i++) { Motor motor = motor_characteristics; motor.dist = dists[i]; - motor.radial_angle = angles[i]; + motor.radial_angle = angles[i] * M_PI / 180.0; motor.direction = rotation_dirs[i]; motor.command = 0.0; motors_[i] = motor; @@ -144,8 +145,6 @@ Eigen::Matrix Multirotor::update_forces_and_torques(CurrentState x double motor_cmd = (act_cmds[i] - 1000)/1000.0; - std::cout << "motor_cmd: " << motor_cmd << "\n"; - motors_[i].command = motor_cmd; } @@ -162,6 +161,8 @@ Eigen::Matrix Multirotor::update_forces_and_torques(CurrentState x double prop_speed = ((-b + sqrt((pow((b), 2.0)) - (4 * a * c))) / (2 * a)); + // std::cout << "angle: " << motor.radial_angle << "\n"; + // Calculate the advance ratio. double advance_ratio = (2. * M_PI * Va.norm())/(prop_speed*motor.prop.diam); @@ -169,36 +170,36 @@ Eigen::Matrix Multirotor::update_forces_and_torques(CurrentState x double motor_thrust = CT * (rho * pow(motor.prop.diam, 4))/(4*pow(M_PI,2)) * pow(prop_speed, 2); - double motor_roll_torque = motor_thrust*motor.dist*sinf(motor.radial_angle); + double motor_roll_torque = -motor_thrust*motor.dist*sinf(motor.radial_angle); double motor_pitch_torque = motor_thrust*motor.dist*cosf(motor.radial_angle); + + // std::cout << "motor_roll_torque " << motor_roll_torque << "\n"; double CQ = motor.prop.CQ_2*pow(advance_ratio, 2) + motor.prop.CQ_1*advance_ratio + motor.prop.CQ_0; + // std::cout << "CQ " << CQ << "\n"; // The torque produced by the propeller spinning. double prop_torque = motor.direction * CQ * (rho*pow(motor.prop.diam, 5))/(4*pow(M_PI,2)) * pow(prop_speed, 2); // TODO: Do you need this? - double motor_yaw_torque = motor_thrust*motor.dist*cosf(motor.radial_angle); - - total_thrust += motor_thrust; + total_thrust += motor_thrust; // Okay total_roll_torque += motor_roll_torque; total_pitch_torque += motor_pitch_torque; - total_yaw_torque += motor_yaw_torque; + total_yaw_torque += prop_torque; // Okay } // Rotate from body to inertial frame. Eigen::Vector3d body_forces; - body_forces << 0.0, 0.0, total_thrust; + body_forces << 0.0, 0.0, -total_thrust; Eigen::Vector3d body_torques; - body_torques << total_roll_torque, total_pitch_torque, total_yaw_torque; - - Eigen::Vector3d inertial_forces = x.rot.inverse() * body_forces; - Eigen::Vector3d inertial_torques = x.rot.inverse() * body_torques; + body_torques << total_roll_torque, total_pitch_torque, -total_yaw_torque; Eigen::Matrix forces; - forces.block<3,1>(0,0) = inertial_forces; - forces.block<3,1>(3,0) = inertial_torques; + forces.block<3,1>(0,0) = body_forces; + forces.block<3,1>(3,0) = body_torques; + + std::cout << forces << std::endl; // TODO: Apply drag. From e4a6ccd6c3a55e18be56458a90f09738b884709a Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Thu, 1 Aug 2024 15:59:49 -0600 Subject: [PATCH 4/8] Added induced drag. The vehicle flies pretty well, however, the vehicle yeets off into infinity at about 300 m/s randomly. It may be connected to when you change commands randomly. The aircraft can also sustain ~420 m/s and never slow down. --- .../src/multirotor_forces_and_moments.cpp | 28 +++++++++++-------- 1 file changed, 17 insertions(+), 11 deletions(-) diff --git a/rosflight_sim/src/multirotor_forces_and_moments.cpp b/rosflight_sim/src/multirotor_forces_and_moments.cpp index 96cedfc2..afdf497c 100644 --- a/rosflight_sim/src/multirotor_forces_and_moments.cpp +++ b/rosflight_sim/src/multirotor_forces_and_moments.cpp @@ -35,7 +35,6 @@ #include #include #include -#include #include namespace rosflight_sim @@ -153,16 +152,19 @@ Eigen::Matrix Multirotor::update_forces_and_torques(CurrentState x // Use model with V_max double v_in = V_max * motor.command; + double Va_norm = Va.norm(); + if (Va_norm < 0.1) { + Va_norm = 0.1; + } + double a = (motor.prop.CQ_0 * (rho) * (pow((motor.prop.diam), 5.0)) / (pow((2 * M_PI), 2.0))); - double b = (motor.prop.CQ_1 * (rho) * (pow((motor.prop.diam), 4.0)) / (2 * M_PI)) * Va.norm() // FIXME: The Va.norm() is actually just the portion parrallel to the prop's thrust. + double b = (motor.prop.CQ_1 * (rho) * (pow((motor.prop.diam), 4.0)) / (2 * M_PI)) * Va_norm // FIXME: The Va.norm() is actually just the portion parrallel to the prop's thrust. + ((pow((motor.KQ), 2.0)) / (motor.R)); double c = (motor.prop.CQ_2 * (rho) * (pow((motor.prop.diam), 3.0)) * (pow(Va.norm(), 2.0))) - ((motor.KQ) / (motor.R)) * v_in + ((motor.KQ) * (motor.I_0)); double prop_speed = ((-b + sqrt((pow((b), 2.0)) - (4 * a * c))) / (2 * a)); - // std::cout << "angle: " << motor.radial_angle << "\n"; - // Calculate the advance ratio. double advance_ratio = (2. * M_PI * Va.norm())/(prop_speed*motor.prop.diam); @@ -172,11 +174,8 @@ Eigen::Matrix Multirotor::update_forces_and_torques(CurrentState x double motor_roll_torque = -motor_thrust*motor.dist*sinf(motor.radial_angle); double motor_pitch_torque = motor_thrust*motor.dist*cosf(motor.radial_angle); - - // std::cout << "motor_roll_torque " << motor_roll_torque << "\n"; double CQ = motor.prop.CQ_2*pow(advance_ratio, 2) + motor.prop.CQ_1*advance_ratio + motor.prop.CQ_0; - // std::cout << "CQ " << CQ << "\n"; // The torque produced by the propeller spinning. double prop_torque = motor.direction * CQ * (rho*pow(motor.prop.diam, 5))/(4*pow(M_PI,2)) * pow(prop_speed, 2); // TODO: Do you need this? @@ -191,6 +190,17 @@ Eigen::Matrix Multirotor::update_forces_and_torques(CurrentState x Eigen::Vector3d body_forces; body_forces << 0.0, 0.0, -total_thrust; + + double Cd = 0.05; + + Eigen::Matrix3d drag_matrix; + drag_matrix << -total_thrust*Cd, 0.0, 0.0, + 0.0, -total_thrust*Cd, 0.0, + 0.0, 0.0, 0.0; + + Eigen::Vector3d drag_forces = drag_matrix * x.vel; + + body_forces += drag_forces; Eigen::Vector3d body_torques; body_torques << total_roll_torque, total_pitch_torque, -total_yaw_torque; @@ -199,10 +209,6 @@ Eigen::Matrix Multirotor::update_forces_and_torques(CurrentState x forces.block<3,1>(0,0) = body_forces; forces.block<3,1>(3,0) = body_torques; - std::cout << forces << std::endl; - - // TODO: Apply drag. - // TODO: Apply ground effect geometry_msgs::msg::TwistStamped msg; From 473b0d58c93e194948543bc23ff24b3e9f86e0bb Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Mon, 5 Aug 2024 10:26:22 -0600 Subject: [PATCH 5/8] Fixed thrust coeffs, updated drag model and mass. Working model that reflects reality relativley well. The params are hardcoded in this commit. --- rosflight_sim/params/multirotor_dynamics.yaml | 2 +- .../src/multirotor_forces_and_moments.cpp | 42 +++++++++++++++---- rosflight_sim/src/rosflight_sil.cpp | 2 +- rosflight_sim/xacro/multirotor.urdf.xacro | 4 +- 4 files changed, 38 insertions(+), 12 deletions(-) diff --git a/rosflight_sim/params/multirotor_dynamics.yaml b/rosflight_sim/params/multirotor_dynamics.yaml index b83c29a3..35d5a0d3 100644 --- a/rosflight_sim/params/multirotor_dynamics.yaml +++ b/rosflight_sim/params/multirotor_dynamics.yaml @@ -34,4 +34,4 @@ V_max : 24.0 # voltage for 6s battery at 4 volts per cell R_motor : 0.042 # ohms I_0 : 1.5 # no-load (zero torque) current (A) - KQ : 0.01705 #((1 /(KV_rpm_per_volt)) *60) / (2 *M_PI) Back-emf constant, KV in V-s/rad, Motor torque constant, KQ in N-m/A + KQ : 0.028937 #((1 /(KV_rpm_per_volt)) *60) / (2 *M_PI) Back-emf constant, KV in V-s/rad, Motor torque constant, KQ in N-m/A diff --git a/rosflight_sim/src/multirotor_forces_and_moments.cpp b/rosflight_sim/src/multirotor_forces_and_moments.cpp index afdf497c..33afb1b1 100644 --- a/rosflight_sim/src/multirotor_forces_and_moments.cpp +++ b/rosflight_sim/src/multirotor_forces_and_moments.cpp @@ -34,7 +34,7 @@ #include #include -#include + #include namespace rosflight_sim @@ -152,24 +152,33 @@ Eigen::Matrix Multirotor::update_forces_and_torques(CurrentState x // Use model with V_max double v_in = V_max * motor.command; - double Va_norm = Va.norm(); - if (Va_norm < 0.1) { - Va_norm = 0.1; + // Take true angle and only compare the portion parrallel to the body z-axis. + Eigen::Vector3d body_z; + body_z << 0.0, 0.0, 1.0; + + double Va_along_z = abs(Va.dot(body_z)); + + if (Va_along_z < 0.001) { + Va_along_z = 0.001; } double a = (motor.prop.CQ_0 * (rho) * (pow((motor.prop.diam), 5.0)) / (pow((2 * M_PI), 2.0))); - double b = (motor.prop.CQ_1 * (rho) * (pow((motor.prop.diam), 4.0)) / (2 * M_PI)) * Va_norm // FIXME: The Va.norm() is actually just the portion parrallel to the prop's thrust. + double b = (motor.prop.CQ_1 * (rho) * (pow((motor.prop.diam), 4.0)) / (2 * M_PI)) * Va_along_z + ((pow((motor.KQ), 2.0)) / (motor.R)); - double c = (motor.prop.CQ_2 * (rho) * (pow((motor.prop.diam), 3.0)) * (pow(Va.norm(), 2.0))) - - ((motor.KQ) / (motor.R)) * v_in + ((motor.KQ) * (motor.I_0)); + double c = (motor.prop.CQ_2 * (rho) * (pow((motor.prop.diam), 3.0)) * (pow(Va_along_z, 2.0))) + - ((motor.KQ) / (motor.R)) * v_in + ((motor.KQ) * (motor.I_0)); // COULD BE OUR CULPRIT double prop_speed = ((-b + sqrt((pow((b), 2.0)) - (4 * a * c))) / (2 * a)); // Calculate the advance ratio. - double advance_ratio = (2. * M_PI * Va.norm())/(prop_speed*motor.prop.diam); + double advance_ratio = (2. * M_PI * Va_along_z)/(prop_speed*motor.prop.diam); double CT = motor.prop.CT_2*pow(advance_ratio, 2) + motor.prop.CT_1*advance_ratio + motor.prop.CT_0; + if (CT <= 0.0) { + CT = 0.0001; + } + double motor_thrust = CT * (rho * pow(motor.prop.diam, 4))/(4*pow(M_PI,2)) * pow(prop_speed, 2); double motor_roll_torque = -motor_thrust*motor.dist*sinf(motor.radial_angle); @@ -177,6 +186,10 @@ Eigen::Matrix Multirotor::update_forces_and_torques(CurrentState x double CQ = motor.prop.CQ_2*pow(advance_ratio, 2) + motor.prop.CQ_1*advance_ratio + motor.prop.CQ_0; + if (CQ <= 0.0) { + CQ = 0.0001; + } + // The torque produced by the propeller spinning. double prop_torque = motor.direction * CQ * (rho*pow(motor.prop.diam, 5))/(4*pow(M_PI,2)) * pow(prop_speed, 2); // TODO: Do you need this? @@ -186,11 +199,24 @@ Eigen::Matrix Multirotor::update_forces_and_torques(CurrentState x total_yaw_torque += prop_torque; // Okay } + double cross_sectional_area = 0.04; // m^2 + double C_d = 2.0; + // Rotate from body to inertial frame. Eigen::Vector3d body_forces; body_forces << 0.0, 0.0, -total_thrust; + double drag = 0.5 * rho * cross_sectional_area * C_d * pow(Va.norm(), 2); + + Eigen::Vector3d drag_force = drag * Va / Va.norm(); + + if (Va.norm() < 0.001) { + drag_force << 0.0, 0.0, 0.0; + } + + body_forces -= drag_force; + double Cd = 0.05; Eigen::Matrix3d drag_matrix; diff --git a/rosflight_sim/src/rosflight_sil.cpp b/rosflight_sim/src/rosflight_sil.cpp index dfff38a5..5a30aea5 100644 --- a/rosflight_sim/src/rosflight_sil.cpp +++ b/rosflight_sim/src/rosflight_sil.cpp @@ -173,7 +173,7 @@ void ROSflightSIL::OnUpdate(const gazebo::common::UpdateInfo & _info) forces_ = mav_dynamics_->update_forces_and_torques(state, board_.get_outputs()); - // apply the forces and torques to the joint (apply in NWU) + // apply the forces and torques to the joint (apply in NWU) FORCES ARE IN NED AND MUST BE CONVERTED TO NWU. GazeboVector force = vec3_to_gazebo_from_eigen(NWU_to_NED * forces_.block<3, 1>(0, 0)); GazeboVector torque = vec3_to_gazebo_from_eigen(NWU_to_NED * forces_.block<3, 1>(3, 0)); link_->AddRelativeForce(force); diff --git a/rosflight_sim/xacro/multirotor.urdf.xacro b/rosflight_sim/xacro/multirotor.urdf.xacro index b83011dc..a5177b07 100644 --- a/rosflight_sim/xacro/multirotor.urdf.xacro +++ b/rosflight_sim/xacro/multirotor.urdf.xacro @@ -14,11 +14,11 @@ Author: James Jackson - + - + /> From 125ca6064dfb7803c8e3d546a09e4e684a0f531c Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Mon, 5 Aug 2024 10:49:28 -0600 Subject: [PATCH 6/8] Added params for drag forces. --- rosflight_sim/params/multirotor_dynamics.yaml | 12 ++++++++-- .../src/multirotor_forces_and_moments.cpp | 24 ++++++++++++------- 2 files changed, 25 insertions(+), 11 deletions(-) diff --git a/rosflight_sim/params/multirotor_dynamics.yaml b/rosflight_sim/params/multirotor_dynamics.yaml index 35d5a0d3..265ccb3d 100644 --- a/rosflight_sim/params/multirotor_dynamics.yaml +++ b/rosflight_sim/params/multirotor_dynamics.yaml @@ -30,8 +30,16 @@ # Diameter (m) D_prop: 0.381 - + + # Motor Specs V_max : 24.0 # voltage for 6s battery at 4 volts per cell R_motor : 0.042 # ohms I_0 : 1.5 # no-load (zero torque) current (A) - KQ : 0.028937 #((1 /(KV_rpm_per_volt)) *60) / (2 *M_PI) Back-emf constant, KV in V-s/rad, Motor torque constant, KQ in N-m/A + KQ : 0.028937 #((1 /(KV_rpm_per_volt)) *60) / (2 *M_PI) Back-emf constant, KV in V-s/rad, Motor torque constant, KQ in N-m/A + + # Drag Model + CD: 2.05 # Coeffecient of drag for a rough cube face on into flow. + A_c: 0.04 # Crossectional Area to use in Drag. + + # Induced Drag Model + CD_induced: 0.05 diff --git a/rosflight_sim/src/multirotor_forces_and_moments.cpp b/rosflight_sim/src/multirotor_forces_and_moments.cpp index 33afb1b1..58bb33ad 100644 --- a/rosflight_sim/src/multirotor_forces_and_moments.cpp +++ b/rosflight_sim/src/multirotor_forces_and_moments.cpp @@ -35,6 +35,7 @@ #include #include +#include #include namespace rosflight_sim @@ -117,7 +118,12 @@ void Multirotor::declare_multirotor_params() node_->declare_parameter("rotor_rotation_directions", rclcpp::PARAMETER_INTEGER_ARRAY); node_->declare_parameter("D_prop", rclcpp::PARAMETER_DOUBLE); - node_->declare_parameter("rho", rclcpp::PARAMETER_DOUBLE); // TODO: make sure these have default parameters. + node_->declare_parameter("rho", rclcpp::PARAMETER_DOUBLE); + + node_->declare_parameter("CD", rclcpp::PARAMETER_DOUBLE); + node_->declare_parameter("A_c", rclcpp::PARAMETER_DOUBLE); + + node_->declare_parameter("CD_induced", rclcpp::PARAMETER_DOUBLE); } Eigen::Matrix Multirotor::update_forces_and_torques(CurrentState x, @@ -199,15 +205,15 @@ Eigen::Matrix Multirotor::update_forces_and_torques(CurrentState x total_yaw_torque += prop_torque; // Okay } - double cross_sectional_area = 0.04; // m^2 - double C_d = 2.0; + double cross_sectional_area = node_->get_parameter("A_c").as_double(); // m^2 + double CD = node_->get_parameter("CD").as_double(); // Rotate from body to inertial frame. Eigen::Vector3d body_forces; body_forces << 0.0, 0.0, -total_thrust; - double drag = 0.5 * rho * cross_sectional_area * C_d * pow(Va.norm(), 2); + double drag = 0.5 * rho * cross_sectional_area * CD * pow(Va.norm(), 2); Eigen::Vector3d drag_force = drag * Va / Va.norm(); @@ -217,16 +223,16 @@ Eigen::Matrix Multirotor::update_forces_and_torques(CurrentState x body_forces -= drag_force; - double Cd = 0.05; + double CD_induced = node_->get_parameter("CD_induced").as_double(); Eigen::Matrix3d drag_matrix; - drag_matrix << -total_thrust*Cd, 0.0, 0.0, - 0.0, -total_thrust*Cd, 0.0, + drag_matrix << -total_thrust*CD_induced, 0.0, 0.0, + 0.0, -total_thrust*CD_induced, 0.0, 0.0, 0.0, 0.0; - Eigen::Vector3d drag_forces = drag_matrix * x.vel; + Eigen::Vector3d induced_drag_force = drag_matrix * x.vel; - body_forces += drag_forces; + body_forces += induced_drag_force; Eigen::Vector3d body_torques; body_torques << total_roll_torque, total_pitch_torque, -total_yaw_torque; From 04cdddf532598a167be506cda104a29eda373225 Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Mon, 5 Aug 2024 11:21:50 -0600 Subject: [PATCH 7/8] Added ground effect. Also pilot tested the sim. The yaw rate control seems slow. --- .../multirotor_forces_and_moments.hpp | 19 ++------------- rosflight_sim/params/multirotor_dynamics.yaml | 3 +++ .../src/multirotor_forces_and_moments.cpp | 24 +++++++++---------- 3 files changed, 17 insertions(+), 29 deletions(-) diff --git a/rosflight_sim/include/rosflight_sim/multirotor_forces_and_moments.hpp b/rosflight_sim/include/rosflight_sim/multirotor_forces_and_moments.hpp index 1adab04a..e2682a72 100644 --- a/rosflight_sim/include/rosflight_sim/multirotor_forces_and_moments.hpp +++ b/rosflight_sim/include/rosflight_sim/multirotor_forces_and_moments.hpp @@ -86,9 +86,9 @@ class Multirotor : public MAVForcesAndMoments double command; // Resistance of motor. double R; - // Torque constant of motor. FIXME: is this the right name? + // Torque constant of motor. double KQ; - // No load current draw of motor. FIXME: is this the right name? + // No load current draw of motor. double I_0; // Prop data Prop prop; @@ -97,21 +97,6 @@ class Multirotor : public MAVForcesAndMoments int num_rotors_; std::vector motors_; - double linear_mu_; - double angular_mu_; - std::vector ground_effect_; - - Eigen::MatrixXd rotor_position_; - Eigen::MatrixXd rotor_plane_normal_; - Eigen::VectorXd rotor_rotation_direction_; - - Eigen::MatrixXd force_allocation_matrix_; - Eigen::MatrixXd torque_allocation_matrix_; - Eigen::VectorXd desired_forces_; - Eigen::VectorXd desired_torques_; - Eigen::VectorXd actual_forces_; - Eigen::VectorXd actual_torques_; - /** * @brief Declares ROS parameters. Must be called in the constructor. */ diff --git a/rosflight_sim/params/multirotor_dynamics.yaml b/rosflight_sim/params/multirotor_dynamics.yaml index 265ccb3d..f23c1ea3 100644 --- a/rosflight_sim/params/multirotor_dynamics.yaml +++ b/rosflight_sim/params/multirotor_dynamics.yaml @@ -43,3 +43,6 @@ # Induced Drag Model CD_induced: 0.05 + + # Ground Effect Model + ground_effect: [ -55.3516, 181.8265, -203.9874, 85.3735, -7.6619 ] diff --git a/rosflight_sim/src/multirotor_forces_and_moments.cpp b/rosflight_sim/src/multirotor_forces_and_moments.cpp index 58bb33ad..bf21d671 100644 --- a/rosflight_sim/src/multirotor_forces_and_moments.cpp +++ b/rosflight_sim/src/multirotor_forces_and_moments.cpp @@ -35,7 +35,6 @@ #include #include -#include #include namespace rosflight_sim @@ -43,15 +42,11 @@ namespace rosflight_sim Multirotor::Multirotor(rclcpp::Node::SharedPtr node) : node_(std::move(node)) , num_rotors_(0) - , linear_mu_(0) - , angular_mu_(0) { declare_multirotor_params(); - num_rotors_ = node_->get_parameter("num_rotors").as_int(); - /* Load Rotor Configuration */ motors_.resize(num_rotors_); @@ -98,8 +93,6 @@ Multirotor::~Multirotor() = default; void Multirotor::declare_multirotor_params() { - // node_->declare_parameter("mass", rclcpp::PARAMETER_DOUBLE); - node_->declare_parameter("num_rotors", rclcpp::PARAMETER_INTEGER); node_->declare_parameter("CT_0", rclcpp::PARAMETER_DOUBLE); @@ -124,6 +117,8 @@ void Multirotor::declare_multirotor_params() node_->declare_parameter("A_c", rclcpp::PARAMETER_DOUBLE); node_->declare_parameter("CD_induced", rclcpp::PARAMETER_DOUBLE); + + node_->declare_parameter("ground_effect", rclcpp::PARAMETER_DOUBLE_ARRAY); } Eigen::Matrix Multirotor::update_forces_and_torques(CurrentState x, @@ -131,10 +126,6 @@ Eigen::Matrix Multirotor::update_forces_and_torques(CurrentState x { // The maximum voltage of the battery. double V_max = node_->get_parameter("V_max").as_double(); - - // double KQ = node_->get_parameter("KQ").as_double(); - // double R_motor = node_->get_parameter("R_motor").as_double(); - // double I_0 = node_->get_parameter("I_0").as_double(); double rho = node_->get_parameter("rho").as_double(); @@ -241,7 +232,16 @@ Eigen::Matrix Multirotor::update_forces_and_torques(CurrentState x forces.block<3,1>(0,0) = body_forces; forces.block<3,1>(3,0) = body_torques; - // TODO: Apply ground effect + // Apply Ground Effect + std::vector ground_effect_coeffs = node_->get_parameter("ground_effect").as_double_array(); + + double ground_effect_force = max(0.0, ground_effect_coeffs[0]*pow(-x.pos.z(), 4) + + ground_effect_coeffs[1]*pow(-x.pos.z(), 3) + + ground_effect_coeffs[2]*pow(-x.pos.z(), 2) + + ground_effect_coeffs[3]*-x.pos.z() + + ground_effect_coeffs[4]); + + forces(2) -= ground_effect_force; geometry_msgs::msg::TwistStamped msg; From f25c5a077b2f27402df57520144536dd513a3492 Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Mon, 5 Aug 2024 11:27:55 -0600 Subject: [PATCH 8/8] Removed TODO --- rosflight_sim/src/multirotor_forces_and_moments.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosflight_sim/src/multirotor_forces_and_moments.cpp b/rosflight_sim/src/multirotor_forces_and_moments.cpp index bf21d671..0d821dcd 100644 --- a/rosflight_sim/src/multirotor_forces_and_moments.cpp +++ b/rosflight_sim/src/multirotor_forces_and_moments.cpp @@ -188,7 +188,7 @@ Eigen::Matrix Multirotor::update_forces_and_torques(CurrentState x } // The torque produced by the propeller spinning. - double prop_torque = motor.direction * CQ * (rho*pow(motor.prop.diam, 5))/(4*pow(M_PI,2)) * pow(prop_speed, 2); // TODO: Do you need this? + double prop_torque = motor.direction * CQ * (rho*pow(motor.prop.diam, 5))/(4*pow(M_PI,2)) * pow(prop_speed, 2); total_thrust += motor_thrust; // Okay total_roll_torque += motor_roll_torque;