Add paper reference in the comments

Co-authored-by: Silvan Fuhrer <silvan@auterion.com>

ROMFS/px4fmu_common/init.d/rc.fw_apps

@@ -18,7 +18,7 @@ fw_att_control start
 fw_mode_manager start
 fw_lat_lon_control start
 airspeed_selector start
-
+fw_wind_estimator start
 #
 # Start attitude control auto-tuner
 #

boards/px4/sitl/default.px4board

@@ -24,6 +24,7 @@ CONFIG_MODULES_FW_MODE_MANAGER=y
 CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_FIGURE_OF_EIGHT=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
+CONFIG_MODULES_FW_WIND_ESTIMATOR=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y

msg/Airflow.msg

@@ -0,0 +1,14 @@
+# Estimated airflow around the vehicle
+#
+# This is published by the fw_wind_estimator module.
+
+uint64 timestamp                 # [us] Time since system start
+uint64 timestamp_sample          # [us] Timestamp of the raw data
+float32 u   			 # [m/s] Body-relative air relative velocity component / X direction
+float32 v        		 # [m/s] Body-relative air relative velocity component / Y direction
+float32 w               	 # [m/s] Body-relative air relative velocity component / Z direction
+float32 windspeed_north	# [m/s] Wind component in north / X direction
+float32 windspeed_east # [m/s] Wind component in east / Y direction
+float32 windspeed_down # [m/s] Wind component in down / Z direction
+
+# TOPICS airflow airflow_groundtruth

msg/CMakeLists.txt

@@ -46,6 +46,7 @@ set(msg_files
 	AdcReport.msg
 	Airspeed.msg
 	AirspeedWind.msg
+	Airflow.msg
 	AutotuneAttitudeControlStatus.msg
 	BatteryInfo.msg
 	ButtonEvent.msg

src/modules/fw_wind_estimator/CMakeLists.txt

@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+
+px4_add_module(
+	MODULE modules__fw_wind_estimator
+	MAIN fw_wind_estimator
+	SRCS
+		FixedwingWindEstimator.cpp
+		FixedwingWindEstimator.hpp
+	DEPENDS
+		px4_work_queue
+	MODULE_CONFIG
+		module.yaml
+	)

src/modules/fw_wind_estimator/FixedwingWindEstimator.cpp

@@ -0,0 +1,292 @@
+/****************************************************************************
+ *
+ *   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 "FixedwingWindEstimator.hpp"
+
+#include <lib/atmosphere/atmosphere.h>
+
+using namespace time_literals;
+using namespace matrix;
+
+using math::constrain;
+using math::interpolate;
+using math::radians;
+
+FixedwingWindEstimator::FixedwingWindEstimator() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
+	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
+{
+	/* fetch initial parameter values */
+	parameters_update();
+
+}
+
+FixedwingWindEstimator::~FixedwingWindEstimator()
+{
+	perf_free(_loop_perf);
+}
+
+bool
+FixedwingWindEstimator::init()
+{
+	if (!_vehicle_angular_velocity_sub.registerCallback()) {
+		PX4_ERR("callback registration failed");
+		return false;
+	}
+
+	return true;
+}
+
+int
+FixedwingWindEstimator::parameters_update()
+{
+	return PX4_OK;
+}
+
+void
+FixedwingWindEstimator::vehicle_land_detected_poll()
+{
+	if (_vehicle_land_detected_sub.updated()) {
+		vehicle_land_detected_s vehicle_land_detected {};
+
+		if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
+			_landed = vehicle_land_detected.landed;
+		}
+	}
+}
+
+void FixedwingWindEstimator::airspeed_poll()
+{
+	airspeed_validated_s airspeed_validated;
+
+	if (_airspeed_validated_sub.update(&airspeed_validated)) {
+		_calibrated_airspeed = math::max(0.5f, airspeed_validated.calibrated_airspeed_m_s);
+		_true_airspeed = math::max(0.5f, airspeed_validated.true_airspeed_m_s);
+	}
+}
+
+
+void
+FixedwingWindEstimator::vehicle_attitude_poll()
+{
+	vehicle_attitude_s vehicle_attitude{};
+
+	if (_vehicle_attitude_sub.update(&vehicle_attitude)) {
+		// get rotation between NED frames
+		_attitude = Quatf(vehicle_attitude.q);
+	}
+}
+
+void
+FixedwingWindEstimator::vehicle_local_position_poll()
+{
+	vehicle_local_position_s vehicle_local_position;
+
+	if (_vehicle_local_position_sub.update(&vehicle_local_position)) {
+		// get rotation between NED frames
+		_local_velocity = Vector3f(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
+	}
+}
+
+void
+FixedwingWindEstimator::vehicle_acceleration_poll()
+{
+	vehicle_acceleration_s vehicle_acceleration;
+
+	if (_vehicle_acceleration_sub.update(&vehicle_acceleration)) {
+		_acceleration = Vector3f(vehicle_acceleration.xyz);
+	}
+}
+
+matrix::Vector3f FixedwingWindEstimator::predictBodyAirVelocity()
+{
+	// Get Aerodynamic coefficients
+	const float wing_area = _param_fw_w_area.get();
+	const float C_B1 = _param_fw_w_c_b1.get();
+	const float C_A0 = _param_fw_w_c_a0.get();
+	const float C_A1 = _param_fw_w_c_a1.get();
+	const float mass = _param_fw_w_mass.get();
+	const float stall_airspeed = _param_fw_airspd_stall.get();
+
+	// compute expected AoA from g-forces:
+	Vector3f body_force = mass * (_acceleration + _attitude.rotateVectorInverse(Vector3f(0.f, 0.f, CONSTANTS_ONE_G)));
+
+	const float speed = fmaxf(_calibrated_airspeed, stall_airspeed);
+	const float dynamic_force = 0.5f * atmosphere::kAirDensitySeaLevelStandardAtmos * powf(speed, 2) * wing_area;
+	float u_approx = _true_airspeed;
+	float v_approx = -body_force(1) * _true_airspeed / (dynamic_force * C_B1);
+	float w_approx = (body_force(2) * _true_airspeed / dynamic_force  + C_A0) / C_A1;
+	Vector3f vel_air(u_approx, v_approx, w_approx);
+	return vel_air;
+}
+
+void FixedwingWindEstimator::Run()
+{
+	if (should_exit()) {
+		_vehicle_angular_velocity_sub.unregisterCallback();
+		exit_and_cleanup();
+		return;
+	}
+
+	perf_begin(_loop_perf);
+
+	// only run controller if angular velocity changed
+	if (_vehicle_angular_velocity_sub.updated() || (hrt_elapsed_time(&_last_run) > 20_ms)) {
+
+		// only update parameters if they changed
+		bool params_updated = _parameter_update_sub.updated();
+
+		// check for parameter updates
+		if (params_updated) {
+			// clear update
+			parameter_update_s pupdate;
+			_parameter_update_sub.copy(&pupdate);
+
+			// update parameters from storage
+			updateParams();
+			parameters_update();
+		}
+
+		float dt = 0.f;
+
+		static constexpr float DT_MIN = 0.002f;
+		static constexpr float DT_MAX = 0.04f;
+
+		vehicle_angular_velocity_s vehicle_angular_velocity{};
+
+		if (_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity)) {
+			dt = math::constrain((vehicle_angular_velocity.timestamp_sample - _last_run) * 1e-6f, DT_MIN, DT_MAX);
+			_last_run = vehicle_angular_velocity.timestamp_sample;
+		}
+
+		if (dt < DT_MIN || dt > DT_MAX) {
+			const hrt_abstime time_now_us = hrt_absolute_time();
+			dt = math::constrain((time_now_us - _last_run) * 1e-6f, DT_MIN, DT_MAX);
+			_last_run = time_now_us;
+		}
+
+		_vehicle_control_mode_sub.update(&_vcontrol_mode);
+
+		vehicle_land_detected_poll();
+		airspeed_poll();
+		vehicle_attitude_poll();
+		vehicle_local_position_poll();
+
+		const float stall_airspeed = _param_fw_airspd_stall.get();
+
+		// Do not compute wind estimate under stall speed
+		if (_calibrated_airspeed > stall_airspeed) {
+			matrix::Vector3f air_velocity_body = predictBodyAirVelocity();
+
+			matrix::Vector3f air_velocity_local = _attitude.rotateVector(air_velocity_body);
+
+			// compute wind from wind triangle
+			matrix::Vector3f wind = _local_velocity - air_velocity_local;
+			airflow_s airflow_msg;
+			airflow_msg.timestamp = hrt_absolute_time();
+			airflow_msg.u = air_velocity_body(0);
+			airflow_msg.v = air_velocity_body(1);
+			airflow_msg.w = air_velocity_body(2);
+			airflow_msg.windspeed_north = wind(0);
+			airflow_msg.windspeed_east = wind(1);
+			airflow_msg.windspeed_down = wind(2);
+			_airflow_pub.publish(airflow_msg);
+		}
+
+		/* if we are in rotary wing mode, do nothing */
+		if (_vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+			perf_end(_loop_perf);
+			return;
+		}
+	}
+
+	// backup schedule
+	ScheduleDelayed(20_ms);
+
+	perf_end(_loop_perf);
+}
+
+int FixedwingWindEstimator::task_spawn(int argc, char *argv[])
+{
+	FixedwingWindEstimator *instance = new FixedwingWindEstimator();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int FixedwingWindEstimator::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int FixedwingWindEstimator::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+fw_wind_estimator is a 3D wind estimator for fixed-wing vehicles.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("fw_rate_control", "controller");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int fw_wind_estimator_main(int argc, char *argv[])
+{
+	return FixedwingWindEstimator::main(argc, argv);
+}

src/modules/fw_wind_estimator/FixedwingWindEstimator.hpp

@@ -0,0 +1,147 @@
+/****************************************************************************
+ *
+ *   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 <lib/rate_control/rate_control.hpp>
+
+#include <drivers/drv_hrt.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/parameters/param.h>
+#include <lib/perf/perf_counter.h>
+#include <matrix/math.hpp>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/tasks.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/airflow.h>
+#include <uORB/topics/airspeed_validated.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_acceleration.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <geo/geo.h> // CONSTANTS_ONE_G
+/*
+3D wind model-based wind estimation for fixed-wing aerial vehicles. The formulation of the model can be found in
+
+- Harms, M., Lim, J., Rohr, D., Rockenbauer, F., Lawrance, N. and Siegwart, R., 2025. Robust Optimization-based Autonomous Dynamic Soaring with a Fixed-Wing UAV. arXiv preprint arXiv:2512.06610.
+*/
+using uORB::SubscriptionData;
+
+using namespace time_literals;
+
+class FixedwingWindEstimator final : public ModuleBase<FixedwingWindEstimator>, public ModuleParams,
+	public px4::ScheduledWorkItem
+{
+public:
+	FixedwingWindEstimator();
+	~FixedwingWindEstimator() override;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	bool init();
+
+private:
+	void Run() override;
+
+	uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};     	// linear acceleration
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
+
+	uORB::Publication<airflow_s> _airflow_pub{ORB_ID(airflow)};
+
+	vehicle_control_mode_s			_vcontrol_mode{};
+	vehicle_status_s			_vehicle_status{};
+
+	perf_counter_t _loop_perf;
+
+	hrt_abstime _last_run{0};
+
+	float _calibrated_airspeed{0.0f};
+	float _true_airspeed{15.0f};
+	matrix::Quatf _attitude{};
+	matrix::Vector3f _acceleration{0.f, 0.f, 0.f};
+	matrix::Vector3f _local_velocity{0.f, 0.f, 0.f};
+
+	bool _landed{true};
+
+	//Vehicle parameters
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::FW_W_MASS>) _param_fw_w_mass,
+		(ParamFloat<px4::params::FW_W_AREA>) _param_fw_w_area,
+		(ParamFloat<px4::params::FW_W_C_B1>) _param_fw_w_c_b1,
+		(ParamFloat<px4::params::FW_W_C_A0>) _param_fw_w_c_a0,
+		(ParamFloat<px4::params::FW_W_C_A1>) _param_fw_w_c_a1,
+		(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall
+	)
+
+	/**
+	 * Update our local parameter cache.
+	 */
+	int parameters_update();
+	void vehicle_land_detected_poll();
+
+	void airspeed_poll();
+	void vehicle_acceleration_poll();
+	void vehicle_attitude_poll();
+	void vehicle_local_position_poll();
+	matrix::Vector3f predictBodyAirVelocity();
+};

src/modules/fw_wind_estimator/Kconfig

@@ -0,0 +1,12 @@
+menuconfig MODULES_FW_WIND_ESTIMATOR
+	bool "fw_wind_estimator"
+	default n
+	---help---
+		Enable support for fw_wind_estimator
+
+menuconfig USER_FW_RATE_CONTROL
+	bool "fw_wind_estimator running as userspace module"
+	default n
+	depends on BOARD_PROTECTED && MODULES_FW_WIND_ESTIMATOR
+	---help---
+		Put fw_wind_estimator in userspace memory

src/modules/fw_wind_estimator/module.yaml

@@ -0,0 +1,50 @@
+module_name: fw_wind_estimator
+parameters:
+- group: 3D Wind Estimator
+  definitions:
+    FW_W_MASS:
+      description:
+        short: Vehicle Mass
+      type: float
+      default: 1.0
+      unit: 'kg'
+      min: 0.0
+      decimal: 3
+      increment: 0.01
+    FW_W_AREA:
+      description:
+        short: Vehicle Mass
+      type: float
+      default: 0.34
+      unit: 'm^2'
+      min: 0.0
+      max: 25
+      decimal: 3
+      increment: 0.01
+    FW_W_C_B1:
+      description:
+        short: Vehicle Aerodynamic coefficient (side slip)
+      type: float
+      default: 1.0
+      min: 0.0
+      max: 4
+      decimal: 3
+      increment: 0.01
+    FW_W_C_A0:
+      description:
+        short: Vehicle Aerodynamic coefficient (zero angle of attack)
+      type: float
+      default: 0.15188
+      min: 0.0
+      max: 4
+      decimal: 3
+      increment: 0.01
+    FW_W_C_A1:
+      description:
+        short: Vehicle Aerodynamic coefficient (angle of attack)
+      type: float
+      default: 5.015
+      min: 0.0
+      max: 10
+      decimal: 3
+      increment: 0.01

src/modules/logger/logged_topics.cpp

@@ -155,6 +155,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("fixed_wing_longitudinal_setpoint");
 	add_topic("longitudinal_control_configuration");
 	add_topic("lateral_control_configuration");
+	add_topic("airflow", 50);
 	add_optional_topic("fixed_wing_lateral_guidance_status", 100);
 	add_optional_topic("fixed_wing_lateral_status", 100);
 	add_optional_topic("fixed_wing_runway_control", 100);
@@ -216,6 +217,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("actuator_servos", 100);
 	add_topic_multi("vehicle_thrust_setpoint", 20, 2);
 	add_topic_multi("vehicle_torque_setpoint", 20, 2);
+	add_topic("airflow_groundtruth", 10);
 
 	// SYS_HITL: default ground truth logging for simulation
 	int32_t sys_hitl = 0;

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -582,6 +582,20 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &msg)
 
 			local_position_groundtruth.timestamp = timestamp;
 			_lpos_ground_truth_pub.publish(local_position_groundtruth);
+
+
+			airflow_s airflow_groundtruth;
+			airflow_groundtruth.timestamp = timestamp;
+			// Body-relateive air velocity
+			matrix::Vector3f body_velocity = matrix::Quatf(vehicle_attitude_groundtruth.q).rotateVectorInverse(matrix::Vector3f(velocity));
+			airflow_groundtruth.u = body_velocity(0);
+			airflow_groundtruth.v = body_velocity(1);
+			airflow_groundtruth.w = body_velocity(2);
+			///TODO: Include wind
+			airflow_groundtruth.windspeed_north = 0.0;
+			airflow_groundtruth.windspeed_east = 0.0;
+			airflow_groundtruth.windspeed_down = 0.0;
+			_airflow_groundtruth_pub.publish(airflow_groundtruth);
 			return;
 		}
 	}

src/modules/simulation/gz_bridge/GZBridge.hpp

@@ -66,6 +66,7 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_odometry.h>
+#include <uORB/topics/airflow.h>
 
 #include <gz/math.hh>
 #include <gz/msgs.hh>
@@ -144,6 +145,7 @@ private:
 	uORB::Publication<distance_sensor_s>          _distance_sensor_pub{ORB_ID(distance_sensor)};
 	uORB::Publication<differential_pressure_s>    _differential_pressure_pub{ORB_ID(differential_pressure)};
 	uORB::Publication<obstacle_distance_s>        _obstacle_distance_pub{ORB_ID(obstacle_distance)};
+	uORB::Publication<airflow_s> 		      _airflow_groundtruth_pub{ORB_ID(airflow_groundtruth)};
 	uORB::Publication<vehicle_angular_velocity_s> _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)};
 	uORB::Publication<vehicle_attitude_s>         _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)};
 	uORB::Publication<vehicle_global_position_s>  _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)};