Add custom gz airspeed plugin and add wind effects (#26018)

* Add px4 custom airspeed sensor

* Format fix

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

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()

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 <gz/plugin/Register.hh>
+#include <gz/msgs/air_speed.pb.h>
+
+using namespace px4;
+
+// Sign function taken from https://stackoverflow.com/a/4609795/8548472
+template <typename T> 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<const sdf::Element> &sdf,
+			 gz::sim::EntityComponentManager &ecm,
+			 gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	const std::string link_name = sdf->Get<std::string>("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<gz::msgs::AirSpeed>(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());
+}

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 <gz/sim/Util.hh>
+#include <gz/sim/Link.hh>
+#include <gz/sim/Model.hh>
+#include <gz/sim/World.hh>
+#include <gz/sim/System.hh>
+#include "gz/sim/components/LinearVelocity.hh"
+#include <gz/msgs/wind.pb.h>
+
+#include <gz/transport/Node.hh>
+
+#include <random>
+
+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<const sdf::Element> &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<float> standard_normal_distribution_;
+
+	float diff_pressure_stddev_{0.01f}; // [hPa]
+	float _alt_home{DEFAULT_HOME_ALT_AMSL};
+};
+} // end namespace px4

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()