From 5df20b8e9b61d3639b0a5c4ce1ba09bdd7edac80 Mon Sep 17 00:00:00 2001 From: Jaeyoung Lim Date: Mon, 1 Dec 2025 08:56:40 -0800 Subject: [PATCH] Add custom gz airspeed plugin and add wind effects (#26018) * Add px4 custom airspeed sensor * Format fix --- .../airframes/4008_gz_advanced_plane | 2 - .../simulation/gz_plugins/CMakeLists.txt | 5 +- .../gz_plugins/airspeed/AirSpeed.cpp | 137 ++++++++++++++++++ .../gz_plugins/airspeed/AirSpeed.hpp | 96 ++++++++++++ .../gz_plugins/airspeed/CMakeLists.txt | 57 ++++++++ 5 files changed, 293 insertions(+), 4 deletions(-) create mode 100644 src/modules/simulation/gz_plugins/airspeed/AirSpeed.cpp create mode 100644 src/modules/simulation/gz_plugins/airspeed/AirSpeed.hpp create mode 100644 src/modules/simulation/gz_plugins/airspeed/CMakeLists.txt diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane b/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane index 69a4ee5e0d..184ed7ba0d 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane @@ -11,8 +11,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=advanced_plane} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_ARSPDSIM 1 - param set-default FW_LND_ANG 8 param set-default FW_PR_FF 0.08 diff --git a/src/modules/simulation/gz_plugins/CMakeLists.txt b/src/modules/simulation/gz_plugins/CMakeLists.txt index 4423a6b139..16bfa6077c 100644 --- a/src/modules/simulation/gz_plugins/CMakeLists.txt +++ b/src/modules/simulation/gz_plugins/CMakeLists.txt @@ -71,11 +71,12 @@ if (gz-transport_FOUND) add_subdirectory(buoyancy) add_subdirectory(spacecraft_thruster) add_subdirectory(motor_failure) + add_subdirectory(airspeed) # Add an alias target for each plugin if (TARGET GstCameraSystem) - add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin GenericMotorModelPlugin BuoyancySystemPlugin GstCameraSystem SpacecraftThrusterModelPlugin MotorFailurePlugin) + add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin GenericMotorModelPlugin BuoyancySystemPlugin GstCameraSystem SpacecraftThrusterModelPlugin MotorFailurePlugin AirSpeedPlugin) else() - add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin GenericMotorModelPlugin BuoyancySystemPlugin SpacecraftThrusterModelPlugin MotorFailurePlugin) + add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin GenericMotorModelPlugin BuoyancySystemPlugin SpacecraftThrusterModelPlugin MotorFailurePlugin AirSpeedPlugin) endif() endif() diff --git a/src/modules/simulation/gz_plugins/airspeed/AirSpeed.cpp b/src/modules/simulation/gz_plugins/airspeed/AirSpeed.cpp new file mode 100644 index 0000000000..8d0c74dab7 --- /dev/null +++ b/src/modules/simulation/gz_plugins/airspeed/AirSpeed.cpp @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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. + * + ****************************************************************************/ + +#include "AirSpeed.hpp" + +#include +#include + +using namespace px4; + +// Sign function taken from https://stackoverflow.com/a/4609795/8548472 +template int sign(T val) +{ + return (T(0) < val) - (val < T(0)); +} + +// Register the plugin +GZ_ADD_PLUGIN( + AirSpeed, + gz::sim::System, + AirSpeed::ISystemPreUpdate, + AirSpeed::ISystemConfigure +) + +void AirSpeed::Configure(const gz::sim::Entity &entity, + const std::shared_ptr &sdf, + gz::sim::EntityComponentManager &ecm, + gz::sim::EventManager &eventMgr) +{ + _entity = entity; + _model = gz::sim::Model(entity); + + const std::string link_name = sdf->Get("link_name"); + _link_entity = _model.LinkByName(ecm, link_name); + std::string model_name = _model.Name(ecm); + + if (!_link_entity) { + throw std::runtime_error("Airspeed::Configure: Link \"" + link_name + "\" was not found. " + "Please ensure that your model contains the corresponding link."); + } + + _link = gz::sim::Link(_link_entity); + + // Needed to report linear & angular velocity + _link.EnableVelocityChecks(ecm, true); + + _world_entity = gz::sim::worldEntity(ecm); + _world = gz::sim::World(_world_entity); + std::string world_name = _world.Name(ecm).value_or("default"); + + std::string airspeed_topic = "/world/" + world_name + "/model/" + model_name + + "/link/airspeed_link/sensor/air_speed/air_speed"; + _pub = _node.Advertise(airspeed_topic); + + std::string wind_topic = "/world/" + world_name + "/wind_info"; + + _node.Subscribe(wind_topic, &AirSpeed::windCallback, this); + + ///TODO: Read sdf for altitude home position +} + +void AirSpeed::PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) +{ + const auto optional_pose = _link.WorldPose(_ecm); + + if (optional_pose.has_value()) { + _vehicle_position = optional_pose.value().Pos(); + _vehicle_attitude = optional_pose.value().Rot(); + + } else { + gzerr << "Unable to get pose" << std::endl; + } + + const auto optional_vel = _link.WorldLinearVelocity(_ecm); + + if (optional_vel.has_value()) { + _vehicle_velocity = optional_vel.value(); + + } else { + gzerr << "Unable to get linear velocity" << std::endl; + } + + // Compute the air density at the local altitude / temperature + const float alt_rel = _vehicle_position.Z(); // Z-component from ENU + const float alt_amsl = (float)_alt_home + alt_rel; + const float temperature_local = TEMPERATURE_MSL - LAPSE_RATE * alt_amsl; + const float density_ratio = powf(TEMPERATURE_MSL / temperature_local, 4.256f); + const float air_density = AIR_DENSITY_MSL / density_ratio; + + // Calculate differential pressure + noise in hPa + const float diff_pressure_noise = standard_normal_distribution_(random_generator_) * diff_pressure_stddev_; + // Body-relateive air velocity + gz::math::Vector3d air_relative_velocity = _vehicle_velocity - _wind_velocity; + gz::math::Vector3d body_velocity = _vehicle_attitude.RotateVectorReverse(air_relative_velocity); + // Calculate differential pressure in hPa + double diff_pressure = sign(body_velocity.X()) * 0.005 * (double)air_density * body_velocity.X() * body_velocity.X() + + (double)diff_pressure_noise; + gz::msgs::AirSpeed airspeed_msg; + airspeed_msg.set_diff_pressure(diff_pressure * 100.0); + _pub.Publish(airspeed_msg); +} + +void AirSpeed::windCallback(const gz::msgs::Wind &msg) +{ + _wind_velocity = gz::math::Vector3d(msg.linear_velocity().x(), msg.linear_velocity().y(), msg.linear_velocity().z()); +} diff --git a/src/modules/simulation/gz_plugins/airspeed/AirSpeed.hpp b/src/modules/simulation/gz_plugins/airspeed/AirSpeed.hpp new file mode 100644 index 0000000000..108ffd47f5 --- /dev/null +++ b/src/modules/simulation/gz_plugins/airspeed/AirSpeed.hpp @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include "gz/sim/components/LinearVelocity.hh" +#include + +#include + +#include + +namespace px4 +{ + +static constexpr float DEFAULT_HOME_ALT_AMSL = 488.0; // altitude AMSL at Irchel Park, Zurich, Switzerland [m] + +// international standard atmosphere (troposphere model - valid up to 11km) see [1] +static constexpr float TEMPERATURE_MSL = 288.15; // temperature at MSL [K] (15 [C]) +static constexpr float PRESSURE_MSL = 101325.0; // pressure at MSL [Pa] +static constexpr float LAPSE_RATE = 0.0065; // reduction in temperature with altitude for troposphere [K/m] +static constexpr float AIR_DENSITY_MSL = 1.225; // air density at MSL [kg/m^3] + +class AirSpeed: + public gz::sim::System, + public gz::sim::ISystemPreUpdate, + public gz::sim::ISystemConfigure +{ +public: + void PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) final; + + void Configure(const gz::sim::Entity &entity, + const std::shared_ptr &sdf, + gz::sim::EntityComponentManager &ecm, + gz::sim::EventManager &eventMgr) override; + void windCallback(const gz::msgs::Wind &msg); + +private: + gz::sim::Entity _entity; + gz::sim::Model _model{gz::sim::kNullEntity}; + gz::sim::Entity _link_entity; + gz::sim::Link _link; + gz::sim::Entity _world_entity; + gz::sim::World _world; + + gz::transport::Node _node; + gz::transport::Node::Publisher _pub; + + gz::math::Quaterniond _vehicle_attitude; + gz::math::Vector3d _vehicle_velocity{0., 0., 0.}; + gz::math::Vector3d _vehicle_position{0., 0., 0.}; + gz::math::Vector3d _wind_velocity{0., 0., 0.}; + + std::default_random_engine random_generator_; + std::normal_distribution standard_normal_distribution_; + + float diff_pressure_stddev_{0.01f}; // [hPa] + float _alt_home{DEFAULT_HOME_ALT_AMSL}; +}; +} // end namespace px4 diff --git a/src/modules/simulation/gz_plugins/airspeed/CMakeLists.txt b/src/modules/simulation/gz_plugins/airspeed/CMakeLists.txt new file mode 100644 index 0000000000..2afd37c86e --- /dev/null +++ b/src/modules/simulation/gz_plugins/airspeed/CMakeLists.txt @@ -0,0 +1,57 @@ +############################################################################ +# +# Copyright (c) 2025 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. +# +############################################################################ + +project(AirSpeedPlugin) + + +add_library(${PROJECT_NAME} SHARED + AirSpeed.cpp +) + +target_link_libraries(${PROJECT_NAME} + PUBLIC px4_gz_msgs + PUBLIC ${GZ_SENSORS_TARGET} + PUBLIC ${GZ_PLUGIN_TARGET} + PUBLIC ${GZ_SIM_TARGET} + PUBLIC ${GZ_TRANSPORT_TARGET} +) + +target_include_directories(${PROJECT_NAME} + PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} + PUBLIC ${CMAKE_CURRENT_BINARY_DIR} + PUBLIC px4_gz_msgs +) + +if (NOT CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT) + install(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION lib/px4_gz_plugins) +endif()