Subscribe to wind

This commit adds a px4 custom airspeed plugin

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -89,6 +89,10 @@ int GZBridge::init()
 		return PX4_ERROR;
 	}
 
+	if (!subscribeWind(false)) {
+		return PX4_ERROR;
+	}
+
 	// OPTIONAL:
 	if (_sim_gz_en_gps.get()) {
 		if (!subscribeNavsat(false)) {
@@ -328,6 +332,18 @@ bool GZBridge::subscribeOpticalFlow(bool required)
 	return true;
 }
 
+bool GZBridge::subscribeWind(bool required)
+{
+	std::string wind_topic = "/world/" + _world_name + "/wind_info";
+
+	if (!_node.Subscribe(wind_topic, &GZBridge::windCallback, this)) {
+		PX4_ERR("failed to subscribe to %s", wind_topic.c_str());
+		return required ? false : true;
+	}
+
+	return true;
+}
+
 void GZBridge::clockCallback(const gz::msgs::Clock &msg)
 {
 	// NOTE: PX4-SITL time needs to stay in sync with gz, so this clock-sync will happen on every callback.
@@ -646,6 +662,18 @@ void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &msg)
 	_visual_odometry_pub.publish(report);
 }
 
+void GZBridge::windCallback(const gz::msgs::Wind &msg)
+{
+	// define quaternion for rotation between ENU to NED
+	static const auto q_ENU_to_NED = gz::math::Quaterniond(0, 0.70711, 0.70711, 0);
+
+	gz::math::Vector3d wind_velocity = q_ENU_to_NED.RotateVector(gz::math::Vector3d(
+			msg.linear_velocity().x(),
+			msg.linear_velocity().y(),
+			msg.linear_velocity().z()));
+	_wind_velocity = matrix::Vector3d(wind_velocity[0], wind_velocity[1], wind_velocity[2]);
+}
+
 float GZBridge::generate_wgn()
 {
 	// generate white Gaussian noise sample with std=1

src/modules/simulation/gz_bridge/GZBridge.hpp

@@ -79,6 +79,8 @@
 #include <gz/msgs/laserscan.pb.h>
 #include <gz/msgs/stringmsg.pb.h>
 #include <gz/msgs/scene.pb.h>
+#include <gz/msgs/wind.pb.h>
+
 // Custom PX4 proto
 #include <opticalflow.pb.h>
 
@@ -119,6 +121,7 @@ private:
 	bool subscribeAirPressure(bool required);
 	bool subscribeNavsat(bool required);
 	bool subscribeOpticalFlow(bool required);
+	bool subscribeWind(bool required);
 
 	void clockCallback(const gz::msgs::Clock &msg);
 	void airspeedCallback(const gz::msgs::AirSpeed &msg);
@@ -131,6 +134,7 @@ private:
 	void laserScanCallback(const gz::msgs::LaserScan &msg);
 	void opticalFlowCallback(const px4::msgs::OpticalFlow &msg);
 	void magnetometerCallback(const gz::msgs::Magnetometer &msg);
+	void windCallback(const gz::msgs::Wind &msg);
 
 	static void rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::math::Quaterniond q_FLU_to_ENU);
 
@@ -170,6 +174,7 @@ private:
 	matrix::Vector3d _velocity_prev{};
 	matrix::Vector3f _euler_prev{};
 	hrt_abstime _timestamp_prev{};
+	matrix::Vector3d _wind_velocity{};
 
 	const std::string _world_name;
 	const std::string _model_name;

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,131 @@
+/****************************************************************************
+ *
+ *   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);
+
+	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;
+	std::string _model_name;
+	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);
+
+	///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
+	///TODO: Subscribe to wind velocity from the world
+	gz::math::Vector3d _wind_velocity{0.0, 0.0, 0.0};
+	gz::math::Vector3d air_relative_velocity = _vehicle_velocity - _wind_velocity;
+	gz::math::Vector3d body_velocity = _vehicle_attitude.RotateVectorReverse(air_relative_velocity);
+	double diff_pressure = sign(body_velocity.X()) * 0.005 * (double)air_density  * body_velocity.X() * body_velocity.X() +
+			       (double)diff_pressure_noise;
+	// Calculate differential pressure in hPa
+	gz::msgs::AirSpeed airspeed_msg;
+	// airspeed_msg.set_time_usec(last_time_.Double() * 1e6);
+	airspeed_msg.set_diff_pressure(diff_pressure);
+	_pub.Publish(airspeed_msg);
+}

src/modules/simulation/gz_plugins/airspeed/AirSpeed.hpp

@@ -0,0 +1,93 @@
+/****************************************************************************
+ *
+ *   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/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;
+
+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.};
+
+	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()