diff --git a/rosflight_sim/include/rosflight_sim/fixedwing_forces_and_moments.hpp b/rosflight_sim/include/rosflight_sim/fixedwing_forces_and_moments.hpp index a8fe447..568d926 100644 --- a/rosflight_sim/include/rosflight_sim/fixedwing_forces_and_moments.hpp +++ b/rosflight_sim/include/rosflight_sim/fixedwing_forces_and_moments.hpp @@ -38,6 +38,7 @@ #include #include #include +#include namespace rosflight_sim { @@ -141,6 +142,8 @@ class Fixedwing : public MAVForcesAndMoments * for any missing parameters. */ void update_params_from_ROS(); + + rclcpp::Publisher::SharedPtr forces_moments_pub_; public: /** diff --git a/rosflight_sim/include/rosflight_sim/sil_board.hpp b/rosflight_sim/include/rosflight_sim/sil_board.hpp index 4fe4c43..dc535a3 100644 --- a/rosflight_sim/include/rosflight_sim/sil_board.hpp +++ b/rosflight_sim/include/rosflight_sim/sil_board.hpp @@ -46,6 +46,7 @@ #include #include +#include #include #include @@ -76,6 +77,9 @@ class SILBoard : public UDPBoard double acc_bias_range_ = 0; double acc_bias_walk_stdev_ = 0; + double mass_ = 0; // Use actual values since these are divisors. + double rho_ = 0; // This will prevent a division by zero. + double baro_bias_walk_stdev_ = 0; double baro_stdev_ = 0; double baro_bias_range_ = 0; @@ -96,6 +100,10 @@ class SILBoard : public UDPBoard double vertical_gps_stdev_ = 0; double gps_velocity_stdev_ = 0; + double f_x = 0; + double f_y = 0; + double f_z = 0; + GazeboVector gyro_bias_; GazeboVector acc_bias_; GazeboVector mag_bias_; @@ -117,6 +125,7 @@ class SILBoard : public UDPBoard rclcpp::Node::SharedPtr node_; rclcpp::Subscription::SharedPtr rc_sub_; + rclcpp::Subscription::SharedPtr forces_sub_; rosflight_msgs::msg::RCRaw latestRC_; bool rc_received_ = false; rclcpp::Time last_rc_message_; @@ -135,6 +144,12 @@ class SILBoard : public UDPBoard * @param msg ROSflight RC message published on /RC topic */ void RC_callback(const rosflight_msgs::msg::RCRaw & msg); + /** + * @brief Callback function to update the aerodynamic forces. + * + * @param msg Geometry message that contains the forces and moments. + */ + void forces_callback(const geometry_msgs::msg::TwistStamped & msg); /** * @brief Checks the current pwm value for throttle to see if the motor pwm is above minimum, and that * the motors should be spinning. diff --git a/rosflight_sim/params/anaconda_dynamics.yaml b/rosflight_sim/params/anaconda_dynamics.yaml index a1d5faf..38b1253 100644 --- a/rosflight_sim/params/anaconda_dynamics.yaml +++ b/rosflight_sim/params/anaconda_dynamics.yaml @@ -11,6 +11,7 @@ /fixedwing/rosflight_sil: ros__parameters: rho: 1.2682 + mass: 4.5 wing_s: .52 wing_b: 2.08 diff --git a/rosflight_sim/params/skyhunter_dynamics.yaml b/rosflight_sim/params/skyhunter_dynamics.yaml index 38d76ee..9edf6c9 100644 --- a/rosflight_sim/params/skyhunter_dynamics.yaml +++ b/rosflight_sim/params/skyhunter_dynamics.yaml @@ -11,6 +11,7 @@ /fixedwing/rosflight_sil: ros__parameters: rho: 1.2682 + mass: 2.28 wing_s: 0.4296 wing_b: 1.795 diff --git a/rosflight_sim/src/fixedwing_forces_and_moments.cpp b/rosflight_sim/src/fixedwing_forces_and_moments.cpp index c1874ed..8072103 100644 --- a/rosflight_sim/src/fixedwing_forces_and_moments.cpp +++ b/rosflight_sim/src/fixedwing_forces_and_moments.cpp @@ -56,6 +56,7 @@ Fixedwing::Fixedwing(rclcpp::Node::SharedPtr node) declare_fixedwing_params(); update_params_from_ROS(); wind_ = Eigen::Vector3d::Zero(); + forces_moments_pub_ = node_->create_publisher("/forces_and_moments", 1); } Fixedwing::~Fixedwing() = default; @@ -63,6 +64,7 @@ Fixedwing::~Fixedwing() = default; void Fixedwing::declare_fixedwing_params() { node_->declare_parameter("rho", rclcpp::PARAMETER_DOUBLE); + node_->declare_parameter("mass", rclcpp::PARAMETER_DOUBLE); node_->declare_parameter("wing_s", rclcpp::PARAMETER_DOUBLE); node_->declare_parameter("wing_b", rclcpp::PARAMETER_DOUBLE); @@ -521,6 +523,22 @@ Eigen::Matrix Fixedwing::update_forces_and_torques(CurrentState x, forces(5) = 0.0; } + geometry_msgs::msg::TwistStamped msg; + + rclcpp::Time now = node_->get_clock()->now(); + + msg.header.stamp = now; + + msg.twist.linear.x = forces(0); + msg.twist.linear.y = forces(1); + msg.twist.linear.z = forces(2); + + msg.twist.angular.x = forces(3); + msg.twist.angular.y = forces(4); + msg.twist.angular.z = forces(5); + + forces_moments_pub_->publish(msg); + return forces; } diff --git a/rosflight_sim/src/sil_board.cpp b/rosflight_sim/src/sil_board.cpp index 2d97577..4de60e1 100644 --- a/rosflight_sim/src/sil_board.cpp +++ b/rosflight_sim/src/sil_board.cpp @@ -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 @@ -32,7 +33,9 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include "rosflight_sim/gz_compat.hpp" #include +#include #include #include @@ -102,6 +105,9 @@ void SILBoard::gazebo_setup(gazebo::physics::LinkPtr link, gazebo::physics::Worl imu_update_rate_ = node_->get_parameter_or("imu_update_rate", 1000.0); imu_update_period_us_ = (uint64_t) (1e6 / imu_update_rate_); + mass_ = node_->get_parameter_or("mass", 2.28); + rho_ = node_->get_parameter_or("rho", 1.225); + // Calculate Magnetic Field Vector (for mag simulation) auto inclination = node_->get_parameter_or("inclination", 1.14316156541); auto declination = node_->get_parameter_or("declination", 0.198584539676); @@ -230,12 +236,15 @@ bool SILBoard::imu_read(float accel[3], float * temperature, float gyro[3], uint GazeboQuaternion q_I_NWU = GZ_COMPAT_GET_ROT(GZ_COMPAT_GET_WORLD_POSE(link_)); GazeboVector current_vel = GZ_COMPAT_GET_RELATIVE_LINEAR_VEL(link_); GazeboVector y_acc; + GazeboPose local_pose = GZ_COMPAT_GET_WORLD_POSE(link_); // this is James's egregious hack to overcome wild imu while sitting on the ground if (GZ_COMPAT_GET_LENGTH(current_vel) < 0.05) { y_acc = q_I_NWU.RotateVectorReverse(-gravity_); - } else { + } else if (local_pose.Z() < 0.5) { y_acc = q_I_NWU.RotateVectorReverse(GZ_COMPAT_GET_WORLD_LINEAR_ACCEL(link_) - gravity_); + } else { + y_acc.Set(f_x/mass_, -f_y/mass_, -f_z/mass_); } // Apply normal noise (only if armed, because most of the noise comes from motors @@ -354,7 +363,7 @@ void SILBoard::baro_read(float * pressure, float * temperature) double alt = GZ_COMPAT_GET_Z(GZ_COMPAT_GET_POS(current_state_NWU)) + origin_altitude_; // Convert to the true pressure reading - double y_baro = 101325.0f * (float) pow((1 - 2.25694e-5 * alt), 5.2553); + double y_baro = 101325.0f * (float) pow((1 - 2.25694e-5 * alt), 5.2553); // Add these parameters to the parameters. // Add noise y_baro += baro_stdev_ * normal_distribution_(random_generator_); @@ -380,7 +389,6 @@ bool SILBoard::diff_pressure_present() void SILBoard::diff_pressure_read(float * diff_pressure, float * temperature) { - static double rho_ = 1.225; // Calculate Airspeed GazeboVector vel = GZ_COMPAT_GET_RELATIVE_LINEAR_VEL(link_); @@ -447,6 +455,17 @@ void SILBoard::pwm_init(uint32_t refresh_rate, uint16_t idle_pwm) rc_sub_ = node_->create_subscription( "RC", 1, std::bind(&SILBoard::RC_callback, this, std::placeholders::_1)); + forces_sub_ = node_->create_subscription( + "/forces_and_moments", 1, std::bind(&SILBoard::forces_callback, this, std::placeholders::_1)); +} + +void SILBoard::forces_callback(const geometry_msgs::msg::TwistStamped & msg) +{ + + f_x = msg.twist.linear.x; + f_y = msg.twist.linear.y; + f_z = msg.twist.linear.z; + } float SILBoard::rc_read(uint8_t channel)