Merge pull request #163 from rosflight/25-imu-sim-update

25 imu sim update

rosflight_sim/include/rosflight_sim/fixedwing_forces_and_moments.hpp

@@ -38,6 +38,7 @@
 #include <eigen3/Eigen/Dense>
 #include <rclcpp/rclcpp.hpp>
 #include <rosflight_sim/mav_forces_and_moments.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
 
 namespace rosflight_sim
 {
@@ -141,6 +142,8 @@ class Fixedwing : public MAVForcesAndMoments
   * for any missing parameters.
   */
   void update_params_from_ROS();
+
+  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr forces_moments_pub_;
 
 public:
   /**

rosflight_sim/include/rosflight_sim/sil_board.hpp

@@ -46,6 +46,7 @@
 #include <gazebo/physics/physics.hh>
 
 #include <rclcpp/rclcpp.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
 #include <rosflight_msgs/msg/rc_raw.hpp>
 
 #include <rosflight_sim/gz_compat.hpp>
@@ -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<rosflight_msgs::msg::RCRaw>::SharedPtr rc_sub_;
+  rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::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.

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

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

rosflight_sim/src/fixedwing_forces_and_moments.cpp

@@ -56,13 +56,15 @@ Fixedwing::Fixedwing(rclcpp::Node::SharedPtr node)
   declare_fixedwing_params();
   update_params_from_ROS();
   wind_ = Eigen::Vector3d::Zero();
+  forces_moments_pub_ = node_->create_publisher<geometry_msgs::msg::TwistStamped>("/forces_and_moments", 1);
 }
 
 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<double, 6, 1> 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;
 }
 

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 <fstream>
+#include <rclcpp/logging.hpp>
 #include <rosflight_sim/sil_board.hpp>
 
 #include <iostream>
@@ -102,6 +105,9 @@ void SILBoard::gazebo_setup(gazebo::physics::LinkPtr link, gazebo::physics::Worl
   imu_update_rate_ = node_->get_parameter_or<double>("imu_update_rate", 1000.0);
   imu_update_period_us_ = (uint64_t) (1e6 / imu_update_rate_);
 
+  mass_ = node_->get_parameter_or<double>("mass", 2.28);
+  rho_ = node_->get_parameter_or<double>("rho", 1.225);
+
   // Calculate Magnetic Field Vector (for mag simulation)
   auto inclination = node_->get_parameter_or<double>("inclination", 1.14316156541);
   auto declination = node_->get_parameter_or<double>("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<rosflight_msgs::msg::RCRaw>(
     "RC", 1, std::bind(&SILBoard::RC_callback, this, std::placeholders::_1));
+  forces_sub_ = node_->create_subscription<geometry_msgs::msg::TwistStamped>(
+    "/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)