diff --git a/.gitignore b/.gitignore index f1dd958cd6..cff3424568 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,6 @@ Build/ build/ +.vscode/ scripts/schemas .DS_Store *~ diff --git a/CMakeLists.txt b/CMakeLists.txt index be7fda8008..ae252c7db7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -374,8 +374,9 @@ add_library(gazebo_barometer_plugin SHARED src/gazebo_barometer_plugin.cpp) add_library(gazebo_catapult_plugin SHARED src/gazebo_catapult_plugin.cpp) add_library(gazebo_usv_dynamics_plugin SHARED src/gazebo_usv_dynamics_plugin.cpp) add_library(gazebo_parachute_plugin SHARED src/gazebo_parachute_plugin.cpp) -add_library(gazebo_airship_dynamics_plugin SHARED src/gazebo_airship_dynamics_plugin.cpp) add_library(gazebo_drop_plugin SHARED src/gazebo_drop_plugin.cpp) +add_library(gazebo_airflowsensor_plugin SHARED src/gazebo_airflowsensor_plugin.cpp) +add_library(gazebo_airship_dynamics_plugin SHARED src/gazebo_airship_dynamics_plugin.cpp) set(plugins gazebo_airspeed_plugin @@ -401,8 +402,9 @@ set(plugins gazebo_catapult_plugin gazebo_usv_dynamics_plugin gazebo_parachute_plugin - gazebo_airship_dynamics_plugin gazebo_drop_plugin + gazebo_airflowsensor_plugin + gazebo_airship_dynamics_plugin ) foreach(plugin ${plugins}) diff --git a/include/gazebo_airflowsensor_plugin.h b/include/gazebo_airflowsensor_plugin.h new file mode 100644 index 0000000000..790bd3f839 --- /dev/null +++ b/include/gazebo_airflowsensor_plugin.h @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. 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 PX4 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 OWNER 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. + * + ****************************************************************************/ +/** + * @brief AirflowSensor Plugin + * + * This plugin publishes Airflow sensor data + * + * @author Henry Kotze + */ + +#ifndef _GAZEBO_AIRFLOWSENSOR_PLUGIN_HH_ +#define _GAZEBO_AIRFLOWSENSOR_PLUGIN_HH_ + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace gazebo +{ + +typedef const boost::shared_ptr WindPtr; + +class GAZEBO_VISIBLE AirflowSensorPlugin : public SensorPlugin +{ +public: + AirflowSensorPlugin(); + virtual ~AirflowSensorPlugin(); + +protected: + virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); + virtual void OnUpdate(const common::UpdateInfo&); + virtual void OnSensorUpdate(); + +private: + void WindVelocityCallback(WindPtr& msg); + + physics::ModelPtr model_; + physics::WorldPtr world_; + physics::LinkPtr link_; + sensors::SensorPtr parentSensor_; + + transport::NodePtr node_handle_; + transport::SubscriberPtr wind_sub_; + transport::PublisherPtr airflow_sensor_pub_; + event::ConnectionPtr updateSensorConnection_; + event::ConnectionPtr updateConnection_; + + // linear velocity + ignition::math::Vector3d vel_a_; + // angular velocity + ignition::math::Vector3d ang_a_; + + common::Time last_time_; + std::string namespace_; + std::string link_name_; + std::string model_name_; + std::string airflowsensor_topic_; + + std::normal_distribution gauss_dir_; + std::normal_distribution gauss_speed_; + std::default_random_engine generator_; + + ignition::math::Vector3d wind_; + ignition::math::Vector3d body_wind_; + ignition::math::Vector3d body_vel_; + float airflow_direction_; + float airflow_speed_; + +}; // class GAZEBO_VISIBLE AirflowSensorPlugin +} // namespace gazebo +#endif // _GAZEBO_AIRFLOWSENSOR_PLUGIN_HH_ diff --git a/include/gazebo_mavlink_interface.h b/include/gazebo_mavlink_interface.h index aec4a4d80d..9d21c80b36 100644 --- a/include/gazebo_mavlink_interface.h +++ b/include/gazebo_mavlink_interface.h @@ -80,6 +80,7 @@ static const std::regex kDefaultGPSModelNaming(".*(gps|ublox-neo-7M)(.*)"); static const std::regex kDefaultAirspeedModelJointNaming(".*(airspeed)(.*_joint)"); static const std::regex kDefaultImuModelJointNaming(".*(imu)(\\d*_joint)"); static const std::regex kDefaultMagModelJointNaming(".*(mag)(\\d*_joint)"); +static const std::regex kDefaultAirflowSensorModelJointNaming(".*(airflowsensor)(.*_joint)"); namespace gazebo { diff --git a/include/mavlink_interface.h b/include/mavlink_interface.h index 40223231f8..b798097115 100644 --- a/include/mavlink_interface.h +++ b/include/mavlink_interface.h @@ -81,7 +81,7 @@ enum class SensorSource { GYRO = 0b111000, MAG = 0b111000000, BARO = 0b1101000000000, - DIFF_PRESS = 0b10000000000, + DIFF_PRESS = 0b10000000000 }; namespace SensorData { @@ -104,6 +104,13 @@ namespace SensorData { struct Airspeed { double diff_pressure; + double speed; + double direction; + }; + + struct WindSensor { + double wind_speed; + double wind_direction; }; struct Gps { @@ -129,12 +136,15 @@ struct HILData { int id=-1; bool baro_updated{false}; bool diff_press_updated{false}; + bool airflow_updated{false}; bool mag_updated{false}; bool imu_updated{false}; double temperature; double pressure_alt; double abs_pressure; double diff_pressure; + double airflow_speed; + double airflow_direction; Eigen::Vector3d mag_b; Eigen::Vector3d accel_b; Eigen::Vector3d gyro_b; @@ -155,6 +165,8 @@ class MavlinkInterface { void SendHeartbeat(); void SendSensorMessages(const uint64_t time_usec); void SendSensorMessages(const uint64_t time_usec, HILData &hil_data); + void SendAirflowSensorMessages(uint64_t time_usec, HILData &hil_data); + void SendGpsMessages(const SensorData::Gps &data); void UpdateBarometer(const SensorData::Barometer &data, const int id = 0); void UpdateAirspeed(const SensorData::Airspeed &data, const int id = 0); diff --git a/models/airflowsensor/airflowsensor.sdf b/models/airflowsensor/airflowsensor.sdf new file mode 100644 index 0000000000..f3edbd4ec9 --- /dev/null +++ b/models/airflowsensor/airflowsensor.sdf @@ -0,0 +1,30 @@ + + + + + 0 0 0 0 0 0 + + + + 0.01 + 0.1 + + + + + + + + 0 0 0 0 0 0 + 5.0 + true + false + + + + + + + diff --git a/models/airflowsensor/model.config b/models/airflowsensor/model.config new file mode 100644 index 0000000000..dd091c7ee3 --- /dev/null +++ b/models/airflowsensor/model.config @@ -0,0 +1,15 @@ + + + airflowsensor + 1.0 + airflowsensor.sdf + + + Henry Kotze + henry@flycloudline.com + + + + FT Technologies ultra sonic wind sensor model + + diff --git a/models/cloudship2/cloudship2.sdf.jinja b/models/cloudship2/cloudship2.sdf.jinja new file mode 100644 index 0000000000..d5c49315c3 --- /dev/null +++ b/models/cloudship2/cloudship2.sdf.jinja @@ -0,0 +1,905 @@ +{# ---------------------------------------------------------------- #} +{# general geometry and properties#} +{# ---------------------------------------------------------------- #} +{%- set log_airship_dynamics = 1 -%} +{%- set log_motor_forces = 1 -%} +{%- set log_fin_forces = 1 -%} +{%- set plot_aerodynamics_forces = 0 -%} + +{# geometry #} +{# hull geometry #} +{%- set hull_length = 14.356 -%} +{%- set hull_max_diameter = 3.338 -%} + +{# hull volume #} +{%- set hull_volume = 87.98 -%} + +{# fineness Ratio #} +{%- set fineness_ratio = 4.5 -%} + +{# center of volume as measured from the nose #} +{%- set hull_cv = 6.798245698288959 -%} + +{# center of gravity as measured from the CV #} +{%- set hull_cg_x = 0.0211 -%} +{%- set hull_cg_y = 0.000 -%} +{%- set hull_cg_z = -0.936 -%} + +{# mass and moment of inertia #} +{%- set mass = 102.85 -%} + +{%- set ixx = 558.8664089 -%} +{%- set iyy = 617.39743597 -%} +{%- set izz = 764.31756395 -%} + +{# material properties #} +{# air density (at MSL 15 deg) #} +{%- set air_density = 1.169 -%} +{# gas density (Helium) #} +{%- set gas_density = 0.167 -%} + +{# actuator properties #} +{# maximum thruster rotational velocity (rad/s) #} +{%- set mot_max_vel = 655 -%} +{# maximum thruster lift (kg) #} +{%- set mot_max_thrust_kg = 20 -%} +{# main thrusters time constant #} +{%- set mot_tau = 0.07 -%} +{# maximum thruster angle (rad) #} +{%- set mot_max_angle = 360 * np.pi / 180 -%} +{# maximum control surface angle (rad) #} +{%- set fin_cs_max_angle = 120 * np.pi / 180 -%} +{# simulation motor slowdown #} +{%- set sim_rotor_slow = 20 -%} +{# maximum tail thruster rotational velocity (rad/s) #} +{%- set tail_mot_max_vel = 655 -%} +{# maximum tail thruster lift (kg) #} +{%- set tail_mot_max_thrust_kg = 15 -%} +{# main tail thrusters time constant #} +{%- set tail_mot_tau = 0.07 -%} +{# thruster motor constant #} +{%- set mot_coeff = (mot_max_thrust_kg * 9.81)/(mot_max_vel**2) -%} +{# tail thruster motor constant #} +{%- set tail_mot_coeff = (tail_mot_max_thrust_kg * 9.81)/(tail_mot_max_vel**2) -%} + +{# actuator geometry #} +{# motor distance from gondola #} +{%- set d_mot_y = 1.5 -%} +{# thruster rod radius #} +{%- set mot_rod_r = 0.03 -%} +{# thruster prop radius (16") #} +{%- set mot_prop_r = 0.2032 -%} +{# tail thruster prop radius (16") #} +{%- set tail_mot_prop_r = 0.2032 -%} +{# thruster rod radius inertia factor #} +{%- set mot_rod_r_inertia_factor = 20 -%} +{# thruster prop height #} +{%- set mot_prop_h = 0.02 -%} + +{# fins #} +{%- set fin_root_chord = 1.872 -%} +{%- set fin_span = 0.98 -%} +{%- set fin_thickness = 0.02 -%} +{%- set fin_cs_chord = 0.410 -%} +{%- set fin_cs_gap = 0.001 -%} + +{# center of fin distance from nose and distance from center of hull #} +{%- set d_fin_x_c = 10.77 -%} +{%- set r_fin_rad = 0.41 -%} +{%- set r_fin_cop_x = 5.10653559 -%} +{%- set r_fin_cop_z = 1.737608 -%} +{%- set angle_fin = 20.13833019 -%} +{%- set angle_cs_diff_fin = 7 -%} + +{# aerodynamic properties #} +{# added mass #} +{%- set m11 = 7.565 -%} +{%- set m22 = 96.366 -%} +{%- set m26 = 32.88 -%} +{%- set m33 = m22 -%} +{%- set m35 = -m26 -%} +{%- set m44 = 13.03 -%} +{%- set m53 = -m26 -%} +{%- set m55 = 236.657 -%} +{%- set m62 = m26 -%} +{%- set m66 = m55 -%} + +{# aerodynamic coefficients #} +{%- set dist_potential_flow = hull_cv - 12.34245577700727 -%} +{%- set force_hull_inviscid_flow_coeff = -3.106982155301005 -%} +{%- set force_hull_viscous_flow_coeff = 2.1059728145144914 -%} +{%- set moment_hull_inviscid_flow_coeff = 20.24930722567859 -%} +{%- set moment_hull_viscous_flow_coeff = -13.341622825927411 -%} +{%- set fin_normal_force_coeff = 1.2981061370780032 -%} +{%- set fin_cs_rad_to_cl = 2*np.pi*0.8*0.75*1.2 -%} +{%- set fin_stall_angle = np.rad2deg(0.6997924350094509) -%} +{%- set axial_drag_coeff = 0.001 -%} + +{# ---------------------------------------------------------------- #} +{# positions #} +{# ---------------------------------------------------------------- #} +{# hull center of volume #} +{%- set origin_x = hull_cv -%} +{%- set origin_y = 0 -%} +{%- set origin_z = 0 -%} + +{# ---------------------------------------------------------------- #} +{# macros #} +{# ---------------------------------------------------------------- #} +{# inertial #} +{%- macro inertial(m, ixx, iyy, izz) -%} + + {{m}} + + {{ixx}} + {{iyy}} + {{izz}} + + +{%- endmacro -%} + +{# inertial offset #} +{%- macro inertial_offset(m, ixx, iyy, izz, x, y, z) -%} + + {{ x }} {{ y }} {{ z }} 0 0 0 + {{m}} + + {{ixx}} + {{iyy}} + {{izz}} + + +{%- endmacro -%} + +{# cylinder #} +{%- macro cylinder(r, h) -%} + + + {{r}} + {{h}} + + +{%- endmacro -%} + +{# box #} +{%- macro box(x, y, z) -%} + + + {{x}} {{y}} {{z}} + + +{%- endmacro -%} + +{# fin plugin #} +{%- macro fin_plugin(name, cop_y, cop_z, u_y, u_z, roll) -%} + + + {{ -r_fin_cop_x }} {{ cop_y }} {{ cop_z }} 0 0 0 + {{ inertial(0.001, 0.001, 0.001, 0.001) }} + + 0 0 0 {{ roll }} 0 0 + {{ box(fin_root_chord, fin_thickness, fin_span) }} + + + + {% if plot_aerodynamics_forces == 1 %} + + {{ name }}_liftforce_visual + Gazebo/Blue + + {% endif %} + + + + + hull + {{ name }} + {{ fin_root_chord/2 }} 0 0 0 0 0 + + 1 0 0 + + -{{ 0 }} + {{ 0 }} + + + 1.0 + + + + + 1 + + + + + + 0.0 + {{ fin_normal_force_coeff / (fin_root_chord * fin_span) }} + 0.0 + 0.0 + {{ np.radians(fin_stall_angle) }} + 0.0 + 0.0 + 0.0 + {{ -r_fin_cop_x }} {{ cop_y }} {{ cop_z }} + {{ fin_root_chord * fin_span }} + {{ air_density }} + 1 0 0 + 0 {{ u_y }} {{ u_z }} + hull + {{ name }}_joint + {% if plot_aerodynamics_forces == 1 %} + {{ name }}_liftforce_visual + {% endif %} + + + +{%- endmacro -%} + +{# control surfaces #} +{%- macro control_surface(name, y, z, roll, pitch, yaw, axis_y, axis_z, u_y, u_z, i_span, i_thickness, i_chord, direction) -%} + + {%- set fin_cs_mass = 0.02 -%} + + {{ hull_cv - d_fin_x_c - fin_root_chord - fin_cs_chord/2 - fin_cs_gap }} {{ y }} {{ z }} 0 {{ pitch }} {{ yaw }} + {{ inertial(fin_cs_mass, fin_cs_mass * (1/12)*(i_span**2 + i_thickness**2), fin_cs_mass * (1/12)*(i_chord**2 + i_span**2), fin_cs_mass * (1/12)*(i_chord**2 + i_thickness**2))|indent(6) }} + + true + false + + + + 0 0 0 {{ roll }} 0 0 + {{ box(fin_cs_chord, fin_thickness, fin_span) }} + + + + {% if plot_aerodynamics_forces == 1 %} + + {{ name }}_liftforce_visual + Gazebo/Blue + + {% endif %} + + + + 0 0 0 {{ roll }} 0 0 + {{ box(fin_cs_chord, fin_thickness, fin_span) }} + + + + + hull + {{ name }} + {{ fin_cs_chord/2 }} 0 0 0 0 0 + + 0 {{ -axis_y }} {{ axis_z }} + + -{{ fin_cs_max_angle/2 }} + {{ fin_cs_max_angle/2 }} + + + 0.1 + + + + + 1 + + + + + + 0.0 + 0.0 + 0.0 + 0.0 + {{ np.radians(fin_stall_angle) }} + 0.0 + 0.0 + 0.0 + {{ hull_cv - d_fin_x_c - fin_root_chord - fin_cs_gap }} {{ y }} {{ z }} + {{ fin_cs_chord * fin_span }} + {{ air_density }} + 1 0 0 + 0 {{ u_y }} {{ u_z }} + hull + + {{ name }}_joint + + {{ direction * fin_cs_rad_to_cl }} + {% if plot_aerodynamics_forces == 1 %} + {{ name }}_liftforce_visual + {% endif %} + + +{%- endmacro -%} + +{# Tilt thruster #} +{%- macro tilt_thruster(motor_num, direction, parent, mot_prop_r, mot_tau, mot_max_vel, mot_coeff, reversible, dist_x, dist_y, dist_z, roll) -%} + + {{ dist_x }} {{ dist_y }} {{ dist_z }} {{ 0 }} {{ 0 }} {{ 0 }} + + 0 0 0 0 0 0 + 0.05 + + 0.0166704 + 0 + 0 + 0.0166704 + 0 + 0.0167604 + + + + 0 0 0 0 0 0 + + + 0.01 + 0.01 + + + + + + + + + + + + + 0 0 0 0 0 0 + + + 0.1 + 0.1 + + + + + + + 1 + + 0 + + + + {{ parent }} + motor_{{ motor_num }} + + 0 1 0 + + -{{ mot_max_angle/2 }} + {{ mot_max_angle/2 }} + + + 1.0 + 0 + 0 + + 1 + + + + + {%- set motor_mass = 0.025 -%} + {{ dist_x }} {{ dist_y }} {{ dist_z }} {{ 0 }} {{ 0 }} 0 + + 0 0 0 0 0 0 + 0.005 + + 9.75e-07 + 0 + 0 + 0.000166704 + 0 + 0.000167604 + + + + + + + {{ mot_prop_r * 2 / 0.25711 }} {{ mot_prop_r * 2 / 0.25711 }} {{ mot_prop_h / 0.00959 }} + model://cloudship2/meshes/prop_{{ direction }}.dae + + + + + + + + + + + {{ mot_prop_h }} + {{ mot_prop_r }} + + + + + + + rotor_{{ motor_num }} + motor_{{ motor_num }} + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + +{# Only to visualize motor spin #} + + + rotor_{{ motor_num }}_joint + rotor_{{ motor_num }} + {{ direction }} + {{ reversible }} + {{ mot_tau }} + {{ mot_tau }} + {{ mot_max_vel }} + {{ mot_coeff }} + {{ 0 }} + 0.000806428 + 0.000001 + /gazebo/command/motor_speed + {{ motor_num }} + /motor_speed/{{ motor_num }} + {{sim_rotor_slow}} + +{%- endmacro -%} + +{# thruster #} +{%- macro thruster(motor_num, direction, parent, mot_prop_r, mot_tau, mot_max_vel, mot_coeff, reversible, dist_x, dist_y, dist_z, roll) -%} + + {%- set motor_mass = 0.0001 -%} + {{ dist_x }} {{ dist_y }} {{ dist_z }} {{ roll }} {{ 0 }} 0 + {{ inertial(motor_mass, motor_mass*mot_prop_h**2, motor_mass*(1/12)*((2*mot_prop_r)**2 + mot_prop_h**2), motor_mass*(1/12)*((2*mot_prop_r)**2 + mot_prop_h**2))|indent(6) }} + + false + false + + + + + + {{ mot_prop_r * 2 / 0.25711 }} {{ mot_prop_r * 2 / 0.25711 }} {{ mot_prop_h / 0.00959 }} + model://cloudship2/meshes/prop_{{ direction }}.dae + + + + + + + + + + + {{ mot_prop_h }} + {{ mot_prop_r }} + + + + + + + rotor_{{ motor_num }} + {{ parent }} + + 0 0 1 + + -1e+16 + 1e+16 + + + 0 + 0 + + + + +{# Only to visualize motor spin #} + + + rotor_{{ motor_num }}_joint + rotor_{{ motor_num }} + {{ direction }} + {{ reversible }} + {{ mot_tau }} + {{ mot_tau }} + {{ mot_max_vel }} + {{ mot_coeff }} + {{ 0 }} + 0.000806428 + 0.000001 + /gazebo/command/motor_speed + {{ motor_num }} + /motor_speed/{{ motor_num }} + {{sim_rotor_slow}} + +{%- endmacro -%} + +{# ---------------------------------------------------------------- #} +{# SDF description #} +{# ---------------------------------------------------------------- #} + + + + + + false + false + + {# Airship #} + + 0 0 0 0 0 0 + {{ inertial_offset(mass, ixx, iyy, izz, hull_cg_x, hull_cg_y, hull_cg_z)|indent(6) }} + + true + false + + + {# Hull #} + + {{ 0 }} {{ origin_y }} {{ origin_z }} -0.523599 0 0 + + + 0.001 0.001 0.001 + model://cloudship2/meshes/cloudship2.stl + + + + + + + + {# Hull Collsion: Made as box around hull #} + + {{ 0 }} {{ 0 }} {{ 0 }} 0 0 0 + + + {{hull_length}} {{hull_max_diameter}} {{hull_max_diameter}} + + + + + + + 0 0 -1.2 0 0 0 + + 0 0 0 0 0 0 + 0.0001 + + 1e-05 + 0 + 0 + 1e-05 + 0 + 1e-05 + + + + + + 0.01 + + + + + + + + + /imu_link + hull + + 1 0 0 + + 0 + 0 + 0 + 0 + + + 0 + 0 + + 1 + + + + {# Airspeed Sensor #} + + model://airspeed + 0 0 0 0 0 0 + airspeed + + + airspeed::link + hull + + + {# Airflow Sensor #} + + model://airflowsensor + 5 0 -3 0 0 + airflowsensor + + + airflowsensor::link + hull + + + {# First GPS #} + + model://gps + 0.1 0 -1.2 0 0 0 + gps + + + gps::link + hull + + + {# Lidar #} + + model://sf10a + 0 0 {{ -hull_max_diameter/2 - 0.01}} 0 0 0 + sf10a + + + sf10a::link + hull + + + {# Control surfaces #} + {%- set cs_angle = np.radians(angle_fin - angle_cs_diff_fin) -%} + {{ control_surface("rudder_top", 0, r_fin_cop_z, 0, 0, 0, 0, 1, 1, 0, fin_span, fin_thickness, fin_cs_chord, 1) }} + {{ control_surface("elevator_sb", -r_fin_cop_z*np.cos(np.pi/6), -r_fin_cop_z*np.sin(np.pi/6), np.pi/3*2, 0, 0, 1*np.cos(np.pi/6), -np.sin(np.pi/6), -np.sin(np.pi/6), np.cos(np.pi/6), fin_thickness, fin_span, fin_cs_chord, 1) }} + {{ control_surface("elevator_port", r_fin_cop_z*np.cos(np.pi/6), -r_fin_cop_z*np.sin(np.pi/6), 2*np.pi/3*2, 0, 0, 1*np.cos(np.pi/6), np.sin(np.pi/6), np.sin(np.pi/6), np.cos(np.pi/6), fin_thickness, fin_span, fin_cs_chord, 1) }} + + {# Starboard Main Thruster #} + {{ tilt_thruster(0, "cw", "hull", 2*tail_mot_prop_r, mot_tau, mot_max_vel, mot_coeff, false, 3.07, 2.5, -0.5, -np.pi/2) }} + {# Port Main Thruster #} + {{ tilt_thruster(1, "ccw", "hull", 2*tail_mot_prop_r, mot_tau, mot_max_vel, mot_coeff, false, 3.07, -2.5, -0.5, -np.pi/2) }} + + {# Yaw tail thruster #} + {{ thruster(7, "cw", "hull", tail_mot_prop_r, tail_mot_tau, tail_mot_max_vel, tail_mot_coeff, true, hull_cv - hull_length - tail_mot_prop_r, 0, 0, np.pi/2) }} + {# Vertical tail thruster #} + {{ thruster(8, "cw", "hull", tail_mot_prop_r, tail_mot_tau, tail_mot_max_vel, tail_mot_coeff, true, hull_cv - hull_length - tail_mot_prop_r - 0.5, 0, 0, 0) }} + + {# ---------------------------------------------------------------- #} + {# Plugins #} + {# ---------------------------------------------------------------- #} + {# Airship Dynamics #} + + + hull + {{ hull_volume }} + {{ fineness_ratio }} + {{ hull_length }} + {{ air_density }} + {{ m11 }} + {{ m22 }} + {{ m26 }} + {{ m33 }} + {{ m35 }} + {{ m44 }} + {{ m53 }} + {{ m55 }} + {{ m62 }} + {{ m66 }} + {{log_airship_dynamics}} + {{ hull_cv }} + {{ dist_potential_flow }} + {{ force_hull_inviscid_flow_coeff }} + {{ force_hull_viscous_flow_coeff }} + {{ moment_hull_inviscid_flow_coeff }} + {{ moment_hull_viscous_flow_coeff }} + {{ axial_drag_coeff }} + world_wind + + {# Fins #} + {{ fin_plugin("fin1", 0, r_fin_cop_z, 1, 0, 0) }} + {{ fin_plugin("fin2", -r_fin_cop_z*np.cos(np.pi/6), -r_fin_cop_z*np.sin(np.pi/6), -np.sin(np.pi/6), np.cos(np.pi/6), np.pi/3*2) }} + {{ fin_plugin("fin3", r_fin_cop_z*np.cos(np.pi/6), -r_fin_cop_z*np.sin(np.pi/6), np.sin(np.pi/6), np.cos(np.pi/6), 2*np.pi/3*2) }} + + {# IMU #} + + + /imu_link + /imu + 0.0003394 + 3.8785e-05 + 1000.0 + 0.0087 + 0.004 + 0.006 + 300.0 + 0.196 + + + {# Magnetometer #} + + + 100 + 0.0004 + 6.4e-06 + 600 + /mag + + + {# Barometer #} + + + 50 + /baro + 0 + + + {# MAVLink #} + + + /imu + /mag + /baro + /sf10a/link/lidar + INADDR_ANY + 4560 + 14560 + false + /dev/ttyACM0 + 921600 + INADDR_ANY + 14550 + + false + false + true + true + /gazebo/command/motor_speed + false + 1 + true + false + + + + 0 + 0 + {{ mot_max_vel }} + 0 + 0 + velocity + rotor_0_joint + + + 1 + 0 + {{ mot_max_vel }} + 0 + 0 + velocity + rotor_1_joint + + + 2 + 0 + {{ mot_max_angle/2 }} + 0 + 0 + position + motor_0_joint + +

10

+ 0 + 0 + 1.0 + -1.0 + {{ mot_max_angle/2 }} + {{ -mot_max_angle/2 }} + + + + 3 + 0 + {{ mot_max_angle/2 }} + 0 + 0 + position + motor_1_joint + +

10

+ 0 + 0 + 1.0 + -1.0 + {{ mot_max_angle/2 }} + {{ -mot_max_angle/2 }} + + + + 4 + 0 + {{ fin_cs_max_angle }} + 0 + 0 + position_kinematic + elevator_port_joint + + + 5 + 0 + {{ fin_cs_max_angle }} + 0 + 0 + position_kinematic + elevator_sb_joint + + + 6 + 0 + {{ fin_cs_max_angle }} + 0 + 0 + position_kinematic + rudder_top_joint + + + 7 + 0 + {{ tail_mot_max_vel }} + 0 + 0 + velocity + rotor_7_joint + + + 8 + 0 + {{ -1.0*tail_mot_max_vel }} + 0 + 0 + velocity + rotor_8_joint + + + + + + + + + + + + diff --git a/msgs/Airspeed.proto b/msgs/Airspeed.proto index 0b7c8f9f94..ae3c98dba5 100644 --- a/msgs/Airspeed.proto +++ b/msgs/Airspeed.proto @@ -5,4 +5,6 @@ message Airspeed { required int64 time_usec = 1; required double diff_pressure = 2; + optional double speed = 3; + optional double direction = 4; } diff --git a/src/gazebo_airflowsensor_plugin.cpp b/src/gazebo_airflowsensor_plugin.cpp new file mode 100644 index 0000000000..31c1a50e34 --- /dev/null +++ b/src/gazebo_airflowsensor_plugin.cpp @@ -0,0 +1,188 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. 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 PX4 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 OWNER 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. + * + ****************************************************************************/ +/** + * @brief WindSensor Plugin + * + * This plugin publishes airflow sensor measurements + * + * @author Henry Kotze + */ + +#include +#include + +namespace gazebo { +GZ_REGISTER_SENSOR_PLUGIN(AirflowSensorPlugin) + +AirflowSensorPlugin::AirflowSensorPlugin() : SensorPlugin(), + airflow_direction_(0.0), + airflow_speed_(0.0f), + gauss_dir_(0.0, 4*(M_PI/180.0)), + gauss_speed_(0.0, 0.3) +{ } + +AirflowSensorPlugin::~AirflowSensorPlugin() +{ + if (updateConnection_) + updateConnection_->~Connection(); +} + +void AirflowSensorPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) +{ + // Get then name of the parent sensor + this->parentSensor_ = std::dynamic_pointer_cast(_parent); + if (!parentSensor_) + gzthrow("AirflowSensorPlugin requires a airflow Sensor as its parent"); + + // Get the root model name + const std::string scopedName = _parent->ParentName(); + link_name_ = scopedName; + std::vector names_splitted; + boost::split(names_splitted, scopedName, boost::is_any_of("::")); + names_splitted.erase(std::remove_if(begin(names_splitted), end(names_splitted), + [](const std::string& name) + { return name.size() == 0; }), end(names_splitted)); + const std::string rootModelName = names_splitted.front(); // The first element is the name of the root model + // the second to the last name is the model name + const std::string parentSensorModelName = names_splitted.rbegin()[1]; + + // store the model name + model_name_ = names_splitted[0]; + + // get airflow sensor topic name + if(_sdf->HasElement("topic")) { + airflowsensor_topic_ = _sdf->GetElement("topic")->Get(); + } else { + // if not set by parameter, get the topic name from the model name + airflowsensor_topic_ = parentSensorModelName; + gzwarn << "[gazebo_airflowsensor_plugin]: " + names_splitted.front() + "::" + names_splitted.rbegin()[1] + + " using airflowsensor topic \"" << parentSensorModelName << "\"\n"; + } + + + // Store the pointer to the model. + world_ = physics::get_world(parentSensor_->WorldName()); + + if (_sdf->HasElement("robotNamespace")) { + namespace_ = _sdf->GetElement("robotNamespace")->Get(); + } else { + gzerr << "[gazebo_airflowsensor_plugin] Please specify a robotNamespace.\n"; + } + + this->node_handle_ = transport::NodePtr(new transport::Node()); + node_handle_->Init(namespace_); + + this->parentSensor_->SetUpdateRate(10.0); + this->parentSensor_->SetActive(false); + updateSensorConnection_ = parentSensor_->ConnectUpdated(boost::bind(&AirflowSensorPlugin::OnSensorUpdate, this)); + this->parentSensor_->SetActive(true); + + // Listen to the update event. This event is broadcast every + // simulation iteration. + updateConnection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&AirflowSensorPlugin::OnUpdate, this, _1)); + + airflow_sensor_pub_ = node_handle_->Advertise("~/" + model_name_ + "/link/" + airflowsensor_topic_, 10); + wind_sub_ = node_handle_->Subscribe("~/world_wind", &AirflowSensorPlugin::WindVelocityCallback, this); + + // Set initial wind as zero + wind_.X() = 0; + wind_.Y() = 0; + wind_.Z() = 0; + +} + +void AirflowSensorPlugin::OnUpdate(const common::UpdateInfo&){ + + #if GAZEBO_MAJOR_VERSION >= 9 + model_ = world_->ModelByName(model_name_); + physics::EntityPtr parentEntity = world_->EntityByName(link_name_); +#else + model_ = world_->GetModel(model_name_); + physics::EntityPtr parentEntity = world_->GetEntity(link_name_); +#endif + link_ = boost::dynamic_pointer_cast(parentEntity); + if (link_ == NULL) + gzthrow("[gazebo_airflowsensor_plugin] Couldn't find specified link \"" << link_name_ << "\"."); + +#if GAZEBO_MAJOR_VERSION >= 9 + common::Time current_time = world_->SimTime(); +#else + common::Time current_time = world_->GetSimTime(); +#endif + +#if GAZEBO_MAJOR_VERSION >= 9 + ignition::math::Pose3d T_W_I = link_->WorldPose(); +#else + ignition::math::Pose3d T_W_I = ignitionFromGazeboMath(link_->GetWorldPose()); +#endif + ignition::math::Quaterniond C_W_I = T_W_I.Rot(); + +#if GAZEBO_MAJOR_VERSION >= 9 + body_wind_ = C_W_I.RotateVectorReverse(wind_); + vel_a_ = link_->RelativeLinearVel() - C_W_I.RotateVectorReverse(wind_); + body_vel_ = link_->RelativeLinearVel(); +#else + body_wind_ = C_W_I.RotateVectorReverse(wind_); + vel_a_ = ignitionFromGazeboMath(link_->GetRelativeLinearVel()) - C_W_I.RotateVectorReverse(wind_); + body_vel_ = ignitionFromGazeboMath(link_->GetRelativeLinearVel()); +#endif + + last_time_ = current_time; + airflow_direction_ = atan2f(vel_a_.Y(),vel_a_.X()) * (180.0/M_PI); + // resolution of 1 degree + airflow_direction_ = round(airflow_direction_) * (M_PI/180.0) + gauss_dir_(generator_); + // airflow sensor cannot measure wind in the z-direction + vel_a_.Z() = 0; + airflow_speed_ = vel_a_.Length()*10.0; + // resolution of 0.1m/s + airflow_speed_ = round(airflow_speed_)/10.0 + gauss_speed_(generator_); +} + +void AirflowSensorPlugin::OnSensorUpdate() { + + sensor_msgs::msgs::Airspeed airflow_sensor_msg; + airflow_sensor_msg.set_time_usec(last_time_.Double() * 1e6); + airflow_sensor_msg.set_diff_pressure(ignition::math::NAN_D); + airflow_sensor_msg.set_direction(airflow_direction_); + airflow_sensor_msg.set_speed(airflow_speed_); + airflow_sensor_pub_->Publish(airflow_sensor_msg); + +} + +void AirflowSensorPlugin::WindVelocityCallback(WindPtr& wind) { + + // Get world wind velocity. + ignition::math::Vector3d wind_world = ignition::math::Vector3d(wind->velocity().x(), wind->velocity().y(), wind->velocity().z()); + +} +} // namespace gazebo diff --git a/src/gazebo_mavlink_interface.cpp b/src/gazebo_mavlink_interface.cpp index 42d34a7902..7885b2f3ab 100644 --- a/src/gazebo_mavlink_interface.cpp +++ b/src/gazebo_mavlink_interface.cpp @@ -445,6 +445,7 @@ void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf CreateSensorSubscription(&GazeboMavlinkInterface::AirspeedCallback, this, joints, nested_model, kDefaultAirspeedModelJointNaming); CreateSensorSubscription(&GazeboMavlinkInterface::ImuCallback, this, joints, nested_model, kDefaultImuModelJointNaming); CreateSensorSubscription(&GazeboMavlinkInterface::MagnetometerCallback, this, joints, nested_model, kDefaultMagModelJointNaming); + CreateSensorSubscription(&GazeboMavlinkInterface::AirspeedCallback, this, joints, nested_model, kDefaultAirflowSensorModelJointNaming); // Publish gazebo's motor_speed message motor_velocity_reference_pub_ = node_handle_->Advertise("~/" + model_->GetName() + motor_velocity_reference_pub_topic_, 1); @@ -1089,6 +1090,8 @@ void GazeboMavlinkInterface::MagnetometerCallback(MagnetometerPtr& mag_msg, cons void GazeboMavlinkInterface::AirspeedCallback(AirspeedPtr& airspeed_msg, const int& id) { SensorData::Airspeed airspeed_data; airspeed_data.diff_pressure = airspeed_msg->diff_pressure(); + airspeed_data.speed = airspeed_msg->speed(); + airspeed_data.direction = airspeed_msg->direction(); mavlink_interface_->UpdateAirspeed(airspeed_data, id); } diff --git a/src/mavlink_interface.cpp b/src/mavlink_interface.cpp index 43bab08b03..f78ec9816e 100644 --- a/src/mavlink_interface.cpp +++ b/src/mavlink_interface.cpp @@ -207,6 +207,9 @@ void MavlinkInterface::SendSensorMessages(uint64_t time_usec) { if (data.baro_updated | data.diff_press_updated | data.mag_updated | data.imu_updated) { SendSensorMessages(time_usec, data); } + if(data.airflow_updated){ + SendAirflowSensorMessages(time_usec, data); + } } } @@ -225,6 +228,24 @@ void MavlinkInterface::SendHeartbeat() { } } +void MavlinkInterface::SendAirflowSensorMessages(uint64_t time_usec, HILData &hil_data) { + + + mavlink_hil_airflow_sensor_t airflow_sensor_msg; + // airflow_sensor_msg.id = data.id; + airflow_sensor_msg.time_usec = time_usec; + airflow_sensor_msg.speed = hil_data.airflow_speed; + airflow_sensor_msg.direction = hil_data.airflow_direction; + + hil_data.airflow_updated = false; + + if (!hil_mode_ || (hil_mode_ && !hil_state_level_)) { + mavlink_message_t msg; + mavlink_msg_hil_airflow_sensor_encode_chan(1, 200, MAVLINK_COMM_0, &msg, &airflow_sensor_msg); + send_mavlink_message(&msg); + } +} + void MavlinkInterface::SendSensorMessages(uint64_t time_usec, HILData &hil_data) { const std::lock_guard lock(sensor_msg_mutex_); @@ -274,6 +295,7 @@ void MavlinkInterface::SendSensorMessages(uint64_t time_usec, HILData &hil_data) data->diff_press_updated = false; } + if (!hil_mode_ || (hil_mode_ && !hil_state_level_)) { mavlink_message_t msg; mavlink_msg_hil_sensor_encode_chan(1, 200, MAVLINK_COMM_0, &msg, &sensor_msg); @@ -326,9 +348,17 @@ void MavlinkInterface::UpdateAirspeed(const SensorData::Airspeed &data, int id) const std::lock_guard lock(sensor_msg_mutex_); for (auto& instance : hil_data_) { if (instance.id == id) { - instance.diff_pressure = data.diff_pressure; - instance.diff_press_updated = true; - return; + + if(!isnan(data.diff_pressure)){ + instance.diff_pressure = data.diff_pressure; + instance.diff_press_updated = true; + return; + } else { + instance.airflow_speed = data.speed; + instance.airflow_direction = data.direction; + instance.airflow_updated = true; + return; + } } } //Register new HIL instance if we have never seen the id