Add path api

Fix

ROMFS/px4fmu_common/init.d/rc.fw_apps

@@ -16,6 +16,7 @@ control_allocator start
 fw_rate_control start
 fw_att_control start
 fw_mode_manager start
+fw_guidance_control start
 fw_lat_lon_control start
 airspeed_selector start
 

ROMFS/px4fmu_common/init.d/rc.vtol_apps

@@ -28,6 +28,7 @@ fi
 fw_rate_control start vtol
 fw_att_control start vtol
 fw_mode_manager start
+fw_guidance_control start
 fw_lat_lon_control start vtol
 fw_autotune_attitude_control start vtol
 

boards/px4/sitl/default.px4board

@@ -21,6 +21,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_MODE_MANAGER=y
+CONFIG_MODULES_FW_GUIDANCE_CONTROL=y
 CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_FIGURE_OF_EIGHT=y
 CONFIG_MODULES_FW_RATE_CONTROL=y

msg/CMakeLists.txt

@@ -108,6 +108,7 @@ set(msg_files
 	GimbalManagerSetAttitude.msg
 	GimbalManagerSetManualControl.msg
 	GimbalManagerStatus.msg
+	GlobalPathSetpoint.msg
 	GpioConfig.msg
 	GpioIn.msg
 	GpioOut.msg

msg/GlobalPathSetpoint.msg

@@ -0,0 +1,16 @@
+# Path setpoint in NED frame
+# Input to Guidance controller
+# Needs to be kinematically consistent and feasible for smooth flight.
+# setting a value to NaN means the state should not be controlled
+
+uint32 MESSAGE_VERSION = 0
+
+uint64 timestamp # time since system start (microseconds)
+
+float64 lat			# Latitude, (degrees)
+float64 lon			# Longitude, (degrees)
+float32 alt			# Altitude AMSL, (meters)
+# NED local world frame
+float32[3] tangent # Unit vector tangent to path
+float height_rate
+float32 curvature

src/modules/fw_guidance_control/CMakeLists.txt

@@ -0,0 +1,49 @@
+############################################################################
+#
+#   Copyright (c) 2015-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.
+#
+############################################################################
+
+px4_add_module(
+	MODULE modules__fw_guidance_control
+	MAIN fw_guidance_control
+	SRCS
+		FixedWingGuidanceControl.cpp
+		FixedWingGuidanceControl.hpp
+		ControllerConfigurationHandler.cpp
+		ControllerConfigurationHandler.hpp
+	DEPENDS
+		npfg
+		SlewRate
+		tecs
+		motion_planning
+		performance_model
+		Sticks
+	)

src/modules/fw_guidance_control/ControllerConfigurationHandler.cpp

@@ -0,0 +1,139 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+
+/**
+ * @file CombinedControllerConfigurationHandler.cpp
+ */
+
+#include "ControllerConfigurationHandler.hpp"
+#include <drivers/drv_hrt.h>
+
+using namespace time_literals;
+
+
+void CombinedControllerConfigurationHandler::update(const hrt_abstime now)
+{
+	_longitudinal_updated = floatValueChanged(_longitudinal_configuration_current_cycle.pitch_min,
+				_longitudinal_publisher.get().pitch_min);
+	_longitudinal_updated |= floatValueChanged(_longitudinal_configuration_current_cycle.pitch_max,
+				 _longitudinal_publisher.get().pitch_max);
+	_longitudinal_updated |= floatValueChanged(_longitudinal_configuration_current_cycle.throttle_min,
+				 _longitudinal_publisher.get().throttle_min);
+	_longitudinal_updated |= floatValueChanged(_longitudinal_configuration_current_cycle.throttle_max,
+				 _longitudinal_publisher.get().throttle_max);
+	_longitudinal_updated |= floatValueChanged(_longitudinal_configuration_current_cycle.speed_weight,
+				 _longitudinal_publisher.get().speed_weight);
+	_longitudinal_updated |= floatValueChanged(_longitudinal_configuration_current_cycle.climb_rate_target,
+				 _longitudinal_publisher.get().climb_rate_target);
+	_longitudinal_updated |= floatValueChanged(_longitudinal_configuration_current_cycle.sink_rate_target,
+				 _longitudinal_publisher.get().sink_rate_target);
+	_longitudinal_updated |= booleanValueChanged(_longitudinal_configuration_current_cycle.enforce_low_height_condition,
+				 _longitudinal_publisher.get().enforce_low_height_condition);
+	_longitudinal_updated |= booleanValueChanged(_longitudinal_configuration_current_cycle.disable_underspeed_protection,
+				 _longitudinal_publisher.get().disable_underspeed_protection);
+
+	_lateral_updated |= floatValueChanged(_lateral_configuration_current_cycle.lateral_accel_max,
+					      _lateral_publisher.get().lateral_accel_max);
+
+	if (_longitudinal_updated || now - _time_last_longitudinal_publish > 1_s) {
+		_longitudinal_configuration_current_cycle.timestamp = now;
+		_longitudinal_publisher.update(_longitudinal_configuration_current_cycle);
+		_time_last_longitudinal_publish = _longitudinal_configuration_current_cycle.timestamp;
+	}
+
+	if (_lateral_updated || now - _time_last_lateral_publish > 1_s) {
+		_lateral_configuration_current_cycle.timestamp = now;
+		_lateral_publisher.update(_lateral_configuration_current_cycle);
+		_time_last_lateral_publish = _lateral_configuration_current_cycle.timestamp;
+	}
+
+	_longitudinal_updated = _lateral_updated = false;
+	_longitudinal_configuration_current_cycle = empty_longitudinal_control_configuration;
+	_lateral_configuration_current_cycle = empty_lateral_control_configuration;
+}
+
+void CombinedControllerConfigurationHandler::setThrottleMax(float throttle_max)
+{
+	_longitudinal_configuration_current_cycle.throttle_max = throttle_max;
+}
+
+void CombinedControllerConfigurationHandler::setThrottleMin(float throttle_min)
+{
+	_longitudinal_configuration_current_cycle.throttle_min = throttle_min;
+}
+
+void CombinedControllerConfigurationHandler::setSpeedWeight(float speed_weight)
+{
+	_longitudinal_configuration_current_cycle.speed_weight = speed_weight;
+}
+
+void CombinedControllerConfigurationHandler::setPitchMin(const float pitch_min)
+{
+	_longitudinal_configuration_current_cycle.pitch_min = pitch_min;
+}
+
+void CombinedControllerConfigurationHandler::setPitchMax(const float pitch_max)
+{
+	_longitudinal_configuration_current_cycle.pitch_max = pitch_max;
+}
+
+void CombinedControllerConfigurationHandler::setClimbRateTarget(float climbrate_target)
+{
+	_longitudinal_configuration_current_cycle.climb_rate_target = climbrate_target;
+}
+
+void CombinedControllerConfigurationHandler::setDisableUnderspeedProtection(bool disable)
+{
+	_longitudinal_configuration_current_cycle.disable_underspeed_protection = disable;
+}
+
+void CombinedControllerConfigurationHandler::setSinkRateTarget(const float sinkrate_target)
+{
+	_longitudinal_configuration_current_cycle.sink_rate_target = sinkrate_target;
+}
+
+void CombinedControllerConfigurationHandler::setLateralAccelMax(float lateral_accel_max)
+{
+	_lateral_configuration_current_cycle.lateral_accel_max = lateral_accel_max;
+}
+
+void CombinedControllerConfigurationHandler::setEnforceLowHeightCondition(bool low_height_condition)
+{
+	_longitudinal_configuration_current_cycle.enforce_low_height_condition = low_height_condition;
+}
+
+void CombinedControllerConfigurationHandler::resetLastPublishTime()
+{
+	_time_last_longitudinal_publish = _time_last_lateral_publish = 0;
+}

src/modules/fw_guidance_control/ControllerConfigurationHandler.hpp

@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+
+/**
+ * @file CombinedControllerConfigurationHandler.hpp
+ */
+#ifndef PX4_CONTROLLERCONFIGURATIONHANDLER_HPP
+#define PX4_CONTROLLERCONFIGURATIONHANDLER_HPP
+
+#include <uORB/topics/longitudinal_control_configuration.h>
+#include <uORB/topics/lateral_control_configuration.h>
+#include <uORB/Publication.hpp>
+#include <lib/mathlib/mathlib.h>
+
+static constexpr longitudinal_control_configuration_s empty_longitudinal_control_configuration = {.timestamp = 0, .pitch_min = NAN, .pitch_max = NAN, .throttle_min = NAN, .throttle_max = NAN, .climb_rate_target = NAN, .sink_rate_target = NAN, .speed_weight = NAN, .enforce_low_height_condition = false, .disable_underspeed_protection = false };
+static constexpr lateral_control_configuration_s empty_lateral_control_configuration = {.timestamp = 0, .lateral_accel_max = NAN};
+
+
+class CombinedControllerConfigurationHandler
+{
+public:
+	CombinedControllerConfigurationHandler() = default;
+	~CombinedControllerConfigurationHandler() = default;
+
+	void update(const hrt_abstime now);
+
+	void setEnforceLowHeightCondition(bool low_height_condition);
+
+	void setThrottleMax(float throttle_max);
+
+	void setThrottleMin(float throttle_min);
+
+	void setSpeedWeight(float speed_weight);
+
+	void setPitchMin(const float pitch_min);
+
+	void setPitchMax(const float pitch_max);
+
+	void setClimbRateTarget(float climbrate_target);
+
+	void setDisableUnderspeedProtection(bool disable);
+
+	void setSinkRateTarget(const float sinkrate_target);
+
+	void setLateralAccelMax(float lateral_accel_max);
+
+	void resetLastPublishTime();
+
+private:
+	bool booleanValueChanged(bool new_value, bool current_value)
+	{
+		return current_value != new_value;
+	}
+
+	bool floatValueChanged(float new_value, float current_value)
+	{
+		bool  diff;
+
+		if (PX4_ISFINITE(new_value)) {
+			diff = PX4_ISFINITE(current_value) ? (fabsf(new_value - current_value) > FLT_EPSILON) : true;
+
+		} else {
+			diff = PX4_ISFINITE(current_value);
+		}
+
+		return diff;
+	}
+
+	bool _lateral_updated{false};
+	bool _longitudinal_updated{false};
+
+	hrt_abstime _time_last_longitudinal_publish{0};
+	hrt_abstime _time_last_lateral_publish{0};
+
+	uORB::PublicationData<lateral_control_configuration_s> _lateral_publisher{ORB_ID(lateral_control_configuration)};
+	uORB::PublicationData<longitudinal_control_configuration_s> _longitudinal_publisher{ORB_ID(longitudinal_control_configuration)};
+
+	lateral_control_configuration_s _lateral_configuration_current_cycle{empty_lateral_control_configuration};
+	longitudinal_control_configuration_s _longitudinal_configuration_current_cycle {empty_longitudinal_control_configuration};
+};
+
+#endif //PX4_CONTROLLERCONFIGURATIONHANDLER_HPP

src/modules/fw_guidance_control/FixedWingGuidanceControl.cpp

@@ -0,0 +1,538 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-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 "FixedWingGuidanceControl.hpp"
+
+#include <px4_platform_common/events.h>
+#include <uORB/topics/longitudinal_control_configuration.h>
+
+using math::constrain;
+using math::max;
+using math::min;
+using math::radians;
+
+using matrix::Dcmf;
+using matrix::Eulerf;
+using matrix::Quatf;
+using matrix::Vector2f;
+using matrix::Vector2d;
+using matrix::Vector3f;
+using matrix::wrap_pi;
+
+const fixed_wing_lateral_setpoint_s empty_lateral_control_setpoint = {.timestamp = 0, .course = NAN, .airspeed_direction = NAN, .lateral_acceleration = NAN};
+const fixed_wing_longitudinal_setpoint_s empty_longitudinal_control_setpoint = {.timestamp = 0, .altitude = NAN, .height_rate = NAN, .equivalent_airspeed = NAN, .pitch_direct = NAN, .throttle_direct = NAN};
+
+FixedWingGuidanceControl::FixedWingGuidanceControl() :
+	ModuleParams(nullptr),
+	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
+	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
+{
+	// limit to 50 Hz
+	_local_pos_sub.set_interval_ms(20);
+
+	_launch_detection_status_pub.advertise();
+	_fixed_wing_lateral_guidance_status_pub.advertise();
+
+	parameters_update();
+}
+
+FixedWingGuidanceControl::~FixedWingGuidanceControl()
+{
+	perf_free(_loop_perf);
+}
+
+bool
+FixedWingGuidanceControl::init()
+{
+	if (!_local_pos_sub.registerCallback()) {
+		PX4_ERR("callback registration failed");
+		return false;
+	}
+
+	return true;
+}
+
+void
+FixedWingGuidanceControl::parameters_update()
+{
+	updateParams();
+
+	_directional_guidance.setPeriod(_param_npfg_period.get());
+	_directional_guidance.setDamping(_param_npfg_damping.get());
+	_directional_guidance.enablePeriodLB(_param_npfg_en_period_lb.get());
+	_directional_guidance.enablePeriodUB(_param_npfg_en_period_ub.get());
+	_directional_guidance.setRollTimeConst(_param_npfg_roll_time_const.get());
+	_directional_guidance.setSwitchDistanceMultiplier(_param_npfg_switch_distance_multiplier.get());
+	_directional_guidance.setPeriodSafetyFactor(_param_npfg_period_safety_factor.get());
+}
+
+void
+FixedWingGuidanceControl::vehicle_control_mode_poll()
+{
+	if (_control_mode_sub.updated()) {
+		_control_mode_sub.copy(&_control_mode);
+	}
+}
+
+void
+FixedWingGuidanceControl::airspeed_poll()
+{
+	airspeed_validated_s airspeed_validated;
+
+	if (_param_fw_use_airspd.get() && _airspeed_validated_sub.update(&airspeed_validated)) {
+
+		// do not use synthetic airspeed as it's for the use here not reliable enough
+		if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)
+		    && airspeed_validated.airspeed_source != airspeed_validated_s::SOURCE_SYNTHETIC) {
+
+			_airspeed_eas = airspeed_validated.calibrated_airspeed_m_s;
+		}
+	}
+
+	// no airspeed updates for one second --> declare invalid
+	// this flag is used for some logic like: exiting takeoff, flaps retraction
+	_airspeed_valid = hrt_elapsed_time(&_time_airspeed_last_valid) < 1_s;
+}
+
+void
+FixedWingGuidanceControl::wind_poll(const hrt_abstime now)
+{
+	if (_wind_sub.updated()) {
+		wind_s wind;
+		_wind_sub.update(&wind);
+
+		// assumes wind is valid if finite
+		_wind_valid = PX4_ISFINITE(wind.windspeed_north)
+			      && PX4_ISFINITE(wind.windspeed_east);
+
+		_time_wind_last_received = now;
+
+		_wind_vel(0) = wind.windspeed_north;
+		_wind_vel(1) = wind.windspeed_east;
+
+	} else {
+		// invalidate wind estimate usage (and correspondingly NPFG, if enabled) after subscription timeout
+		_wind_valid = _wind_valid && (now - _time_wind_last_received) < WIND_EST_TIMEOUT;
+	}
+
+	if (!_wind_valid) {
+		_wind_vel(0) = 0.f;
+		_wind_vel(1) = 0.f;
+	}
+}
+
+void
+FixedWingGuidanceControl::manual_control_setpoint_poll()
+{
+	_sticks.checkAndUpdateStickInputs();
+
+	_manual_control_setpoint_for_height_rate = _sticks.getPitch();
+	_manual_control_setpoint_for_airspeed = _sticks.getThrottleZeroCentered();
+
+	if (_param_fw_pos_stk_conf.get() & STICK_CONFIG_SWAP_STICKS_BIT) {
+		/* Alternate stick allocation (similar concept as for multirotor systems:
+		 * demanding up/down with the throttle stick, and move faster/break with the pitch one.
+		 */
+
+		_manual_control_setpoint_for_height_rate = -_sticks.getThrottleZeroCentered();
+		_manual_control_setpoint_for_airspeed = _sticks.getPitch();
+	}
+}
+
+void
+FixedWingGuidanceControl::vehicle_attitude_poll()
+{
+	vehicle_attitude_s vehicle_attitude;
+
+	if (_vehicle_attitude_sub.update(&vehicle_attitude)) {
+		vehicle_angular_velocity_s angular_velocity{};
+		_vehicle_angular_velocity_sub.copy(&angular_velocity);
+		const Vector3f rates{angular_velocity.xyz};
+
+		Dcmf R{Quatf(vehicle_attitude.q)};
+
+		// if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset
+		// between multirotor and fixed wing flight
+		if (_vehicle_status.is_vtol_tailsitter) {
+			const Dcmf R_offset{Eulerf{0.f, M_PI_2_F, 0.f}};
+			R = R * R_offset;
+
+			_yawrate = rates(0);
+
+		} else {
+			_yawrate = rates(2);
+		}
+
+		const Eulerf euler_angles(R);
+		_yaw = euler_angles(2);
+
+		const Vector3f body_acceleration = R.transpose() * Vector3f{_local_pos.ax, _local_pos.ay, _local_pos.az};
+		_body_acceleration_x = body_acceleration(0);
+
+		const Vector3f body_velocity = R.transpose() * Vector3f{_local_pos.vx, _local_pos.vy, _local_pos.vz};
+		_body_velocity_x = body_velocity(0);
+	}
+}
+
+void
+FixedWingGuidanceControl::update_in_air_states(const hrt_abstime now)
+{
+	/* reset flag when airplane landed */
+	if (_landed) {
+		_completed_manual_takeoff = false;
+	}
+}
+
+void
+FixedWingGuidanceControl::control_auto_path(const float control_interval,
+		const Vector2f &ground_speed, const float cruising_speed, const Vector2f curr_wp_local, const float curr_wp_alt,
+		const Vector2f velocity_2d, bool gliding_enabled)
+{
+	const float target_airspeed = cruising_speed > FLT_EPSILON ? cruising_speed : NAN;
+
+	Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
+
+	// Navigate directly on position setpoint and path tangent
+	const float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 /
+				_pos_sp_triplet.current.loiter_radius :
+				0.0f;
+	const DirectionalGuidanceOutput sp = navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(),
+					     ground_speed, _wind_vel, curvature);
+
+	fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint};
+	fw_lateral_ctrl_sp.timestamp = hrt_absolute_time();
+	fw_lateral_ctrl_sp.course = sp.course_setpoint;
+	fw_lateral_ctrl_sp.lateral_acceleration = sp.lateral_acceleration_feedforward;
+	_lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp);
+
+	const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = {
+		.timestamp = hrt_absolute_time(),
+		.altitude = curr_wp_alt,
+		.height_rate = NAN,
+		.equivalent_airspeed = target_airspeed,
+		.pitch_direct = NAN,
+		.throttle_direct = NAN
+	};
+
+	_longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp);
+
+	if (gliding_enabled) {
+		_ctrl_configuration_handler.setThrottleMin(0.0f);
+		_ctrl_configuration_handler.setThrottleMax(0.0f);
+		_ctrl_configuration_handler.setSpeedWeight(2.0f);
+	}
+}
+
+void
+FixedWingGuidanceControl::Run()
+{
+	if (should_exit()) {
+		_local_pos_sub.unregisterCallback();
+		exit_and_cleanup();
+		return;
+	}
+
+	perf_begin(_loop_perf);
+
+	_vehicle_status_sub.update(&_vehicle_status);
+
+	/* only run controller if position changed and we are not running an external mode*/
+
+	const bool is_external_nav_state = ((_vehicle_status.nav_state >= vehicle_status_s::NAVIGATION_STATE_EXTERNAL1)
+					    && (_vehicle_status.nav_state <= vehicle_status_s::NAVIGATION_STATE_EXTERNAL8))
+					   || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
+
+	if (!is_external_nav_state) {
+		// this will cause the configuration handler to publish immediately the next time an internal flight
+		// mode is active
+		_ctrl_configuration_handler.resetLastPublishTime();
+
+	} else if (_local_pos_sub.update(&_local_pos)) {
+
+		const hrt_abstime now = _local_pos.timestamp;
+
+		const float control_interval = math::constrain((now - _last_time_position_control_called) * 1e-6f,
+					       MIN_AUTO_TIMESTEP, MAX_AUTO_TIMESTEP);
+		_last_time_position_control_called = now;
+
+		// check for parameter updates
+		if (_parameter_update_sub.updated()) {
+			// clear update
+			parameter_update_s pupdate;
+			_parameter_update_sub.copy(&pupdate);
+
+			// update parameters from storage
+			parameters_update();
+		}
+
+		vehicle_global_position_s gpos;
+
+		airspeed_poll();
+		manual_control_setpoint_poll();
+		vehicle_attitude_poll();
+		vehicle_control_mode_poll();
+		wind_poll(now);
+
+
+		if (_global_pos_sub.update(&gpos)) {
+			_current_latitude = gpos.lat;
+			_current_longitude = gpos.lon;
+		}
+
+		if (_local_pos.z_global && PX4_ISFINITE(_local_pos.ref_alt)) {
+			_reference_altitude = _local_pos.ref_alt;
+
+		} else {
+			_reference_altitude = 0.f;
+		}
+
+		_current_altitude = -_local_pos.z + _reference_altitude; // Altitude AMSL in meters
+
+		// handle estimator reset events. we only adjust setpoins for manual modes
+		if (_control_mode.flag_control_manual_enabled) {
+			// adjust navigation waypoints in position control mode
+			if (_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_velocity_enabled
+			    && _local_pos.xy_reset_counter != _xy_reset_counter) {
+
+				// reset heading hold flag, which will re-initialise position control
+				_hdg_hold_enabled = false;
+			}
+		}
+
+		// Convert Local setpoints to global setpoints
+		if (!_global_local_proj_ref.isInitialized()
+		    || (_global_local_proj_ref.getProjectionReferenceTimestamp() != _local_pos.ref_timestamp)
+		    || (_local_pos.xy_reset_counter != _xy_reset_counter)) {
+
+			double reference_latitude = 0.;
+			double reference_longitude = 0.;
+
+			if (_local_pos.xy_global && PX4_ISFINITE(_local_pos.ref_lat) && PX4_ISFINITE(_local_pos.ref_lon)) {
+				reference_latitude = _local_pos.ref_lat;
+				reference_longitude = _local_pos.ref_lon;
+			}
+
+			_global_local_proj_ref.initReference(reference_latitude, reference_longitude,
+							     _local_pos.ref_timestamp);
+		}
+
+		if (_control_mode.flag_control_offboard_enabled) {
+			global_trajectory_setpoint_s global_trajectory_setpoint;
+
+			if (_global_trajectory_setpoint_sub.update(&global_trajectory_setpoint)) {
+				bool valid_setpoint = false;
+				_pos_sp_triplet = {}; // clear any existing
+				_pos_sp_triplet.timestamp = global_trajectory_setpoint.timestamp;
+				_pos_sp_triplet.current.timestamp = global_trajectory_setpoint.timestamp;
+				_pos_sp_triplet.current.cruising_speed = NAN; // ignored
+				_pos_sp_triplet.current.cruising_throttle = NAN; // ignored
+				_pos_sp_triplet.current.vx = NAN;
+				_pos_sp_triplet.current.vy = NAN;
+				_pos_sp_triplet.current.vz = NAN;
+				_pos_sp_triplet.current.lat = static_cast<double>(NAN);
+				_pos_sp_triplet.current.lon = static_cast<double>(NAN);
+				_pos_sp_triplet.current.alt = NAN;
+
+				if (PX4_ISFINITE(global_trajectory_setpoint.lat) && PX4_ISFINITE(global_trajectory_setpoint.lon)
+				    && PX4_ISFINITE(global_trajectory_setpoint.alt)) {
+					valid_setpoint = true;
+					_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
+					_pos_sp_triplet.current.lat = global_trajectory_setpoint.lat;
+					_pos_sp_triplet.current.lon = global_trajectory_setpoint.lon;
+					_pos_sp_triplet.current.alt = global_trajectory_setpoint.alt;
+				}
+
+				if (Vector3f(global_trajectory_setpoint.velocity).isAllFinite()) {
+					valid_setpoint = true;
+					_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
+					_pos_sp_triplet.current.vx = global_trajectory_setpoint.velocity[0];
+					_pos_sp_triplet.current.vy = global_trajectory_setpoint.velocity[1];
+					_pos_sp_triplet.current.vz = global_trajectory_setpoint.velocity[2];
+
+					if (Vector3f(global_trajectory_setpoint.acceleration).isAllFinite()) {
+						Vector2f velocity_sp_2d(global_trajectory_setpoint.velocity[0], global_trajectory_setpoint.velocity[1]);
+						Vector2f normalized_velocity_sp_2d = velocity_sp_2d.normalized();
+						Vector2f acceleration_sp_2d(global_trajectory_setpoint.acceleration[0], global_trajectory_setpoint.acceleration[1]);
+						Vector2f acceleration_normal = acceleration_sp_2d - acceleration_sp_2d.dot(normalized_velocity_sp_2d) *
+									       normalized_velocity_sp_2d;
+						float direction = -normalized_velocity_sp_2d.cross(acceleration_normal.normalized());
+						_pos_sp_triplet.current.loiter_radius = direction * velocity_sp_2d.norm() * velocity_sp_2d.norm() /
+											acceleration_normal.norm();
+
+					} else {
+						_pos_sp_triplet.current.loiter_radius = NAN;
+					}
+				}
+
+				_position_setpoint_current_valid = valid_setpoint;
+			}
+		}
+
+		if (_vehicle_land_detected_sub.updated()) {
+			vehicle_land_detected_s vehicle_land_detected;
+
+			if (_vehicle_land_detected_sub.update(&vehicle_land_detected)) {
+				_landed = vehicle_land_detected.landed;
+			}
+		}
+
+		if (!_vehicle_status.in_transition_mode) {
+			// reset position of backtransition start if not in transition
+			_lpos_where_backtrans_started = Vector2f(NAN, NAN);
+			_backtrans_heading = NAN;
+		}
+
+
+		Vector2d curr_pos(_current_latitude, _current_longitude);
+		Vector2f ground_speed(_local_pos.vx, _local_pos.vy);
+
+		update_in_air_states(now);
+
+		// restore nominal TECS parameters in case changed intermittently (e.g. in landing handling)
+
+		// restore lateral-directional guidance parameters (changed in takeoff mode)
+		_directional_guidance.setPeriod(_param_npfg_period.get());
+
+		// by default set speed weight to the param value, can be overwritten inside the methods below
+		_ctrl_configuration_handler.setSpeedWeight(_param_t_spdweight.get());
+
+		Vector2f curr_wp_local = _global_local_proj_ref.project(_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon);
+		const matrix::Vector2f velocity_2d(_pos_sp_triplet.current.vx, _pos_sp_triplet.current.vy);
+		control_auto_path(control_interval, ground_speed, _pos_sp_triplet.current.cruising_speed, curr_wp_local, _pos_sp_triplet.current.alt,
+				  velocity_2d,
+				  _pos_sp_triplet.current.gliding_enabled);
+
+		_ctrl_configuration_handler.update(now);
+
+		// only publish status in full FW mode
+		if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
+		    || _vehicle_status.in_transition_mode) {
+			publish_lateral_guidance_status(now);
+
+		}
+
+		_xy_reset_counter = _local_pos.xy_reset_counter;
+
+		perf_end(_loop_perf);
+	}
+}
+
+
+DirectionalGuidanceOutput FixedWingGuidanceControl::navigatePathTangent(const matrix::Vector2f &vehicle_pos,
+		const matrix::Vector2f &position_setpoint,
+		const matrix::Vector2f &tangent_setpoint,
+		const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature)
+{
+	if (tangent_setpoint.norm() <= FLT_EPSILON) {
+		// degenerate case: no direction. maintain the last npfg command.
+		return DirectionalGuidanceOutput{};
+	}
+
+	const Vector2f unit_path_tangent{tangent_setpoint.normalized()};
+	_closest_point_on_path = position_setpoint;
+	return _directional_guidance.guideToPath(vehicle_pos, ground_vel, wind_vel, tangent_setpoint.normalized(),
+			position_setpoint,
+			curvature);
+}
+
+void FixedWingGuidanceControl::publish_lateral_guidance_status(const hrt_abstime now)
+{
+	fixed_wing_lateral_guidance_status_s fixed_wing_lateral_guidance_status{};
+
+	fixed_wing_lateral_guidance_status.timestamp = now;
+	fixed_wing_lateral_guidance_status.course_setpoint = _directional_guidance.getCourseSetpoint();
+	fixed_wing_lateral_guidance_status.lateral_acceleration_ff = _directional_guidance.getLateralAccelerationSetpoint();
+	fixed_wing_lateral_guidance_status.bearing_feas = _directional_guidance.getBearingFeasibility();
+	fixed_wing_lateral_guidance_status.bearing_feas_on_track = _directional_guidance.getBearingFeasibilityOnTrack();
+	fixed_wing_lateral_guidance_status.signed_track_error = _directional_guidance.getSignedTrackError();
+	fixed_wing_lateral_guidance_status.track_error_bound = _directional_guidance.getTrackErrorBound();
+	fixed_wing_lateral_guidance_status.adapted_period = _directional_guidance.getAdaptedPeriod();
+	fixed_wing_lateral_guidance_status.wind_est_valid = _wind_valid;
+
+	_fixed_wing_lateral_guidance_status_pub.publish(fixed_wing_lateral_guidance_status);
+}
+
+int FixedWingGuidanceControl::task_spawn(int argc, char *argv[])
+{
+	FixedWingGuidanceControl *instance = new FixedWingGuidanceControl();
+
+	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 FixedWingGuidanceControl::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int FixedWingGuidanceControl::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+This implements the setpoint generation for all PX4-internal fixed-wing modes, height-rate control and higher.
+It takes the current mode state of the vehicle as input and outputs setpoints consumed by the fixed-wing
+lateral-longitudinal controller and and controllers below that (attitude, rate).
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("fw_mode_manager", "controller");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int fw_guidance_control_main(int argc, char *argv[])
+{
+	return FixedWingGuidanceControl::main(argc, argv);
+}

src/modules/fw_guidance_control/FixedWingGuidanceControl.hpp

@@ -0,0 +1,428 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-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.
+ *
+ ****************************************************************************/
+
+
+/**
+ * @file FixedWingGuidanceControl.hpp
+ * Implementation of various fixed-wing control modes.
+ */
+
+#ifndef FIXEDWINGGUIDANCECONTROL_HPP_
+#define FIXEDWINGGUIDANCECONTROL_HPP_
+
+#include "ControllerConfigurationHandler.hpp"
+
+#include <float.h>
+#include <drivers/drv_hrt.h>
+#include <lib/geo/geo.h>
+#include <lib/atmosphere/atmosphere.h>
+#include <lib/npfg/DirectionalGuidance.hpp>
+#include <lib/mathlib/mathlib.h>
+#include <lib/perf/perf_counter.h>
+#include <lib/slew_rate/SlewRate.hpp>
+#include <lib/sticks/Sticks.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/px4_work_queue/WorkItem.hpp>
+#include <uORB/uORB.h>
+
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/airspeed_validated.h>
+#include <uORB/topics/fixed_wing_lateral_setpoint.h>
+#include <uORB/topics/fixed_wing_lateral_guidance_status.h>
+#include <uORB/topics/fixed_wing_longitudinal_setpoint.h>
+#include <uORB/topics/fixed_wing_runway_control.h>
+#include <uORB/topics/launch_detection_status.h>
+#include <uORB/topics/normalized_unsigned_setpoint.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/position_controller_status.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/global_trajectory_setpoint.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/wind.h>
+#include <uORB/topics/orbit_status.h>
+
+using namespace time_literals;
+
+using matrix::Vector2d;
+using matrix::Vector2f;
+
+// [m] initial distance of waypoint in front of plane in heading hold mode
+static constexpr float HDG_HOLD_DIST_NEXT = 3000.0f;
+
+// [rad/s] max yawrate at which plane locks yaw for heading hold mode
+static constexpr float HDG_HOLD_YAWRATE_THRESH = 0.15f;
+
+// [.] max manual roll/yaw normalized input from user which does not change the locked heading
+static constexpr float HDG_HOLD_MAN_INPUT_THRESH = 0.01f;
+
+// [us] time after which we abort landing if terrain estimate is not valid. this timer start whenever the terrain altitude
+// was previously valid, and has changed to invalid.
+static constexpr hrt_abstime TERRAIN_ALT_TIMEOUT = 1_s;
+
+// [us] within this timeout, if a distance sensor measurement not yet made, the land waypoint altitude is used for terrain
+// altitude. this timer starts at the beginning of the landing glide slope.
+static constexpr hrt_abstime TERRAIN_ALT_FIRST_MEASUREMENT_TIMEOUT = 10_s;
+
+// [.] max throttle from user which will not lead to motors spinning up in altitude controlled modes
+static constexpr float THROTTLE_THRESH = -.9f;
+
+// [us] time after which the wind estimate is disabled if no longer updating
+static constexpr hrt_abstime WIND_EST_TIMEOUT = 10_s;
+
+// [s] minimum time step between auto control updates
+static constexpr float MIN_AUTO_TIMESTEP = 0.01f;
+
+// [s] maximum time step between auto control updates
+static constexpr float MAX_AUTO_TIMESTEP = 0.05f;
+
+// [rad] minimum pitch while airspeed has not yet reached a controllable value in manual position controlled takeoff modes
+static constexpr float MIN_PITCH_DURING_MANUAL_TAKEOFF = 0.0f;
+
+// [m] arbitrary buffer altitude added to clearance altitude setpoint during takeoff to ensure aircraft passes the clearance
+// altitude while waiting for navigator to flag it exceeded
+static constexpr float kClearanceAltitudeBuffer = 10.0f;
+
+// [m/s] maximum rate at which the touchdown position can be nudged
+static constexpr float MAX_TOUCHDOWN_POSITION_NUDGE_RATE = 4.0f;
+
+// [.] normalized deadzone threshold for manual nudging input
+static constexpr float MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE = 0.15f;
+
+// [s] time interval after touchdown for ramping in runway clamping constraints (touchdown is assumed at FW_LND_TD_TIME after start of flare)
+static constexpr float POST_TOUCHDOWN_CLAMP_TIME = 0.5f;
+
+// [] Stick deadzon
+static constexpr float kStickDeadBand = 0.06f;
+
+class FixedWingGuidanceControl final : public ModuleBase<FixedWingGuidanceControl>, public ModuleParams,
+	public px4::WorkItem
+{
+public:
+	FixedWingGuidanceControl();
+	~FixedWingGuidanceControl() 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 _local_pos_sub{this, ORB_ID(vehicle_local_position)};
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
+	uORB::Subscription _wind_sub{ORB_ID(wind)};
+	uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)};
+	uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)};
+	uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)};
+	uORB::Subscription _global_trajectory_setpoint_sub{ORB_ID(global_trajectory_setpoint)};
+	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
+	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+
+	uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)};
+	uORB::Publication<launch_detection_status_s> _launch_detection_status_pub{ORB_ID(launch_detection_status)};
+	uORB::PublicationMulti<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};
+	uORB::PublicationData<fixed_wing_lateral_setpoint_s> _lateral_ctrl_sp_pub{ORB_ID(fixed_wing_lateral_setpoint)};
+	uORB::PublicationData<fixed_wing_longitudinal_setpoint_s> _longitudinal_ctrl_sp_pub{ORB_ID(fixed_wing_longitudinal_setpoint)};
+	uORB::Publication<fixed_wing_lateral_guidance_status_s> _fixed_wing_lateral_guidance_status_pub{ORB_ID(fixed_wing_lateral_guidance_status)};
+
+	position_setpoint_triplet_s _pos_sp_triplet{};
+	vehicle_control_mode_s _control_mode{};
+	vehicle_local_position_s _local_pos{};
+	vehicle_status_s _vehicle_status{};
+
+	CombinedControllerConfigurationHandler _ctrl_configuration_handler;
+
+	Vector2f _lpos_where_backtrans_started;
+
+	bool _position_setpoint_previous_valid{false};
+	bool _position_setpoint_current_valid{false};
+	bool _position_setpoint_next_valid{false};
+
+	perf_counter_t _loop_perf; // loop performance counter
+
+	// [us] Last absolute time position control has been called
+	hrt_abstime _last_time_position_control_called{0};
+
+	uint8_t _position_sp_type{0};
+
+	enum StickConfig {
+		STICK_CONFIG_SWAP_STICKS_BIT = (1 << 0),
+		STICK_CONFIG_ENABLE_AIRSPEED_SP_MANUAL_BIT = (1 << 1)
+	};
+
+
+	Sticks _sticks{this};
+
+	// VEHICLE STATES
+
+	double _current_latitude{0};
+	double _current_longitude{0};
+	float _current_altitude{0.f};
+
+	float _yaw{0.0f};
+	float _yawrate{0.0f};
+
+	float _body_acceleration_x{0.f};
+	float _body_velocity_x{0.f};
+
+	MapProjection _global_local_proj_ref{};
+
+	float _reference_altitude{NAN}; // [m AMSL] altitude of the local projection reference point
+
+	bool _landed{true};
+
+	// MANUAL MODES
+
+	// indicates whether we have completed a manual takeoff in a position control mode
+	bool _completed_manual_takeoff{false};
+
+	// [rad] yaw setpoint for manual position mode heading hold
+	float _hdg_hold_yaw{0.0f};
+
+	bool _hdg_hold_enabled{false}; // heading hold enabled
+	bool _yaw_lock_engaged{false}; // yaw is locked for heading hold
+
+	Vector2f _hdg_hold_position{}; // position where heading hold started
+
+	// [.] normalized setpoint for manual altitude control [-1,1]; -1,0,1 maps to min,zero,max height rate commands
+	float _manual_control_setpoint_for_height_rate{0.0f};
+
+	// [.] normalized setpoint for manual airspeed control [-1,1]; -1,0,1 maps to min,cruise,max airspeed commands
+	float _manual_control_setpoint_for_airspeed{0.0f};
+
+	// [m/s] airspeed setpoint for manual modes commanded via MAV_CMD_DO_CHANGE_SPEED
+	float _commanded_manual_airspeed_setpoint{NAN};
+
+	// AUTO TAKEOFF
+
+	// [m] ground altitude AMSL where the plane was launched
+	float _takeoff_ground_alt{0.0f};
+
+	// true if a launch, specifically using the launch detector, has been detected
+	bool _launch_detected{false};
+
+	// [deg] global position of the vehicle at the time launch is detected (using launch detector) or takeoff is started (runway)
+	Vector2d _takeoff_init_position{0, 0};
+
+	// [rad] current vehicle yaw at the time the launch is detected
+	float _launch_current_yaw{0.f};
+
+	bool _skipping_takeoff_detection{false};
+
+	// AUTO LANDING
+	// [m] lateral touchdown position offset manually commanded during landing
+	float _lateral_touchdown_position_offset{0.0f};
+
+	// [m] last terrain estimate which was valid
+	float _last_valid_terrain_alt_estimate{0.0f};
+
+	// [us] time at which we had last valid terrain alt
+	hrt_abstime _last_time_terrain_alt_was_valid{0};
+
+	// AIRSPEED
+
+	float _airspeed_eas{0.f};
+	bool _airspeed_valid{false};
+
+	// [us] last time airspeed was received. used to detect timeouts.
+	hrt_abstime _time_airspeed_last_valid{0};
+
+	// WIND
+
+	// [m/s] wind velocity vector
+	Vector2f _wind_vel{0.0f, 0.0f};
+
+	bool _wind_valid{false};
+
+	hrt_abstime _time_wind_last_received{0}; // [us]
+
+	// VTOL / TRANSITION
+	matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN};
+	float _backtrans_heading{NAN};	// used to lock the initial heading for backtransition with no position control
+
+	// ESTIMATOR RESET COUNTERS
+	uint8_t _xy_reset_counter{0};
+	uint64_t _time_last_xy_reset{0};
+
+	// LATERAL-DIRECTIONAL GUIDANCE
+
+	// CLosest point on path to track
+	matrix::Vector2f _closest_point_on_path;
+
+	// nonlinear path following guidance - lateral-directional position control
+	DirectionalGuidance _directional_guidance;
+
+	hrt_abstime _time_in_fixed_bank_loiter{0}; // [us]
+	float _min_current_sp_distance_xy{FLT_MAX};
+
+	// Update our local parameter cache.
+	void parameters_update();
+
+	// Update subscriptions
+	void airspeed_poll();
+
+	void manual_control_setpoint_poll();
+	void vehicle_attitude_poll();
+	void vehicle_control_mode_poll();
+
+	void wind_poll(const hrt_abstime now);
+
+	/**
+	 * @brief Vehicle control for following a path.
+	 *
+	 * @param control_interval Time since last position control call [s]
+	 * @param curr_pos Current 2D local position vector of vehicle [m]
+	 * @param ground_speed Local 2D ground speed of vehicle [m/s]
+	 * @param pos_sp_prev previous position setpoint
+	 * @param pos_sp_curr current position setpoint
+	 */
+	void control_auto_path(const float control_interval, const Vector2f &ground_speed,
+			       const float cruising_speed, const Vector2f curr_wp_local, const float curr_wp_alt, const Vector2f velocity_2d, bool gliding_enabled);
+
+	void publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint);
+
+	/**
+	 * @brief Updates timing information for landed and in-air states.
+	 *
+	 * @param now Current system time [us]
+	 */
+	void update_in_air_states(const hrt_abstime now);
+
+	/*
+	 * Path following logic. Takes poisiton, path tangent, curvature and
+	 * then updates control setpoints to follow a path setpoint.
+	 *
+	 * TODO: deprecate this function with a proper API to NPFG.
+	 *
+	 * @param[in] vehicle_pos vehicle_pos Vehicle position in local coordinates. (N,E) [m]
+	 * @param[in] position_setpoint closest point on a path in local coordinates. (N,E) [m]
+	 * @param[in] tangent_setpoint unit tangent vector of the path [m]
+	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
+	 * @param[in] wind_vel Wind velocity vector [m/s]
+	 * @param[in] curvature of the path setpoint [1/m]
+	 */
+	DirectionalGuidanceOutput navigatePathTangent(const matrix::Vector2f &vehicle_pos,
+			const matrix::Vector2f &position_setpoint,
+			const matrix::Vector2f &tangent_setpoint,
+			const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature);
+
+	void publish_lateral_guidance_status(const hrt_abstime now);
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::FW_R_LIM>) _param_fw_r_lim,
+
+		(ParamFloat<px4::params::NPFG_PERIOD>) _param_npfg_period,
+		(ParamFloat<px4::params::NPFG_DAMPING>) _param_npfg_damping,
+		(ParamBool<px4::params::NPFG_LB_PERIOD>) _param_npfg_en_period_lb,
+		(ParamBool<px4::params::NPFG_UB_PERIOD>) _param_npfg_en_period_ub,
+		(ParamFloat<px4::params::NPFG_ROLL_TC>) _param_npfg_roll_time_const,
+		(ParamFloat<px4::params::NPFG_SW_DST_MLT>) _param_npfg_switch_distance_multiplier,
+		(ParamFloat<px4::params::NPFG_PERIOD_SF>) _param_npfg_period_safety_factor,
+
+		(ParamFloat<px4::params::FW_LND_AIRSPD>) _param_fw_lnd_airspd,
+		(ParamFloat<px4::params::FW_LND_ANG>) _param_fw_lnd_ang,
+		(ParamFloat<px4::params::FW_LND_FL_PMAX>) _param_fw_lnd_fl_pmax,
+		(ParamFloat<px4::params::FW_LND_FL_PMIN>) _param_fw_lnd_fl_pmin,
+		(ParamFloat<px4::params::FW_LND_FLALT>) _param_fw_lnd_flalt,
+		(ParamBool<px4::params::FW_LND_EARLYCFG>) _param_fw_lnd_earlycfg,
+		(ParamInt<px4::params::FW_LND_USETER>) _param_fw_lnd_useter,
+
+		(ParamFloat<px4::params::FW_P_LIM_MAX>) _param_fw_p_lim_max,
+		(ParamFloat<px4::params::FW_P_LIM_MIN>) _param_fw_p_lim_min,
+		(ParamFloat<px4::params::FW_T_CLMB_R_SP>) _param_climbrate_target,
+		(ParamFloat<px4::params::FW_T_SINK_R_SP>) _param_sinkrate_target,
+		(ParamFloat<px4::params::FW_THR_IDLE>) _param_fw_thr_idle,
+		(ParamFloat<px4::params::FW_THR_MAX>) _param_fw_thr_max,
+		(ParamFloat<px4::params::FW_THR_MIN>) _param_fw_thr_min,
+		(ParamFloat<px4::params::FW_FLAPS_LND_SCL>) _param_fw_flaps_lnd_scl,
+		(ParamFloat<px4::params::FW_FLAPS_TO_SCL>) _param_fw_flaps_to_scl,
+		(ParamFloat<px4::params::FW_SPOILERS_LND>) _param_fw_spoilers_lnd,
+		(ParamInt<px4::params::FW_POS_STK_CONF>) _param_fw_pos_stk_conf,
+		(ParamInt<px4::params::FW_GPSF_LT>) _param_nav_gpsf_lt,
+		(ParamFloat<px4::params::FW_GPSF_R>) _param_nav_gpsf_r,
+		(ParamFloat<px4::params::FW_T_SPDWEIGHT>) _param_t_spdweight,
+
+		// external parameters
+		(ParamBool<px4::params::FW_USE_AIRSPD>) _param_fw_use_airspd,
+		(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad,
+		(ParamFloat<px4::params::FW_TKO_PITCH_MIN>) _takeoff_pitch_min,
+		(ParamFloat<px4::params::NAV_FW_ALT_RAD>) _param_nav_fw_alt_rad,
+		(ParamFloat<px4::params::FW_WING_SPAN>) _param_fw_wing_span,
+		(ParamFloat<px4::params::FW_WING_HEIGHT>) _param_fw_wing_height,
+		(ParamBool<px4::params::RWTO_NUDGE>) _param_rwto_nudge,
+		(ParamFloat<px4::params::FW_LND_FL_TIME>) _param_fw_lnd_fl_time,
+		(ParamFloat<px4::params::FW_LND_FL_SINK>) _param_fw_lnd_fl_sink,
+		(ParamFloat<px4::params::FW_LND_TD_TIME>) _param_fw_lnd_td_time,
+		(ParamFloat<px4::params::FW_LND_TD_OFF>) _param_fw_lnd_td_off,
+		(ParamInt<px4::params::FW_LND_NUDGE>) _param_fw_lnd_nudge,
+		(ParamInt<px4::params::FW_LND_ABORT>) _param_fw_lnd_abort,
+		(ParamFloat<px4::params::FW_TKO_AIRSPD>) _param_fw_tko_airspd,
+		(ParamFloat<px4::params::RWTO_PSP>) _param_rwto_psp,
+		(ParamBool<px4::params::FW_LAUN_DETCN_ON>) _param_fw_laun_detcn_on,
+		(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
+		(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
+		(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim,
+		(ParamFloat<px4::params::FW_T_CLMB_MAX>) _param_fw_t_clmb_max
+	)
+};
+
+#endif // FIXEDWINGGUIDANCECONTROL_HPP_

src/modules/fw_guidance_control/Kconfig

@@ -0,0 +1,12 @@
+menuconfig MODULES_FW_GUIDANCE_CONTROL
+	bool "fw_guidance_control"
+	default n
+	---help---
+		Enable support for fw_guidance_control
+
+menuconfig USER_FW_MODE_MANAGER
+	bool "fw_guidance_control running as userspace module"
+	default n
+	depends on BOARD_PROTECTED && MODULES_FW_MODE_MANAGER
+	---help---
+		Put fw_guidance_control in userspace memory

src/modules/fw_guidance_control/fw_guidance_control_params.c

@@ -0,0 +1,33 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-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.
+ *
+ ****************************************************************************/
+

src/modules/fw_mode_manager/FixedWingModeManager.cpp

@@ -373,22 +373,8 @@ FixedWingModeManager::set_control_mode_current(const hrt_abstime &now)
 	_skipping_takeoff_detection = false;
 	const bool doing_backtransition = _vehicle_status.in_transition_mode && !_vehicle_status.in_transition_to_fw;
 
-	if (_control_mode.flag_control_offboard_enabled && _position_setpoint_current_valid
-	    && _control_mode.flag_control_position_enabled) {
-		if (PX4_ISFINITE(_pos_sp_triplet.current.vx) && PX4_ISFINITE(_pos_sp_triplet.current.vy)
-		    && PX4_ISFINITE(_pos_sp_triplet.current.vz)) {
-			// Offboard position with velocity setpoints
-			_control_mode_current = FW_POSCTRL_MODE_AUTO_PATH;
-			return;
-
-		} else {
-			// Offboard position setpoint only
-			_control_mode_current = FW_POSCTRL_MODE_AUTO;
-			return;
-		}
-
-	} else if ((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled)
-		   && _position_setpoint_current_valid) {
+	if ((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled)
+	    && _position_setpoint_current_valid) {
 
 		// Enter this mode only if the current waypoint has valid 3D position setpoints.
 
@@ -1033,47 +1019,6 @@ void FixedWingModeManager::publishFigureEightStatus(const position_setpoint_s po
 }
 #endif // CONFIG_FIGURE_OF_EIGHT
 
-void
-FixedWingModeManager::control_auto_path(const float control_interval, const Vector2d &curr_pos,
-					const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr)
-{
-	const float target_airspeed = pos_sp_curr.cruising_speed > FLT_EPSILON ? pos_sp_curr.cruising_speed : NAN;
-
-	Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
-	Vector2f curr_wp_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);
-
-	// Navigate directly on position setpoint and path tangent
-	const matrix::Vector2f velocity_2d(pos_sp_curr.vx, pos_sp_curr.vy);
-	const float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 /
-				_pos_sp_triplet.current.loiter_radius :
-				0.0f;
-	const DirectionalGuidanceOutput sp = navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(),
-					     ground_speed, _wind_vel, curvature);
-
-	fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint};
-	fw_lateral_ctrl_sp.timestamp = hrt_absolute_time();
-	fw_lateral_ctrl_sp.course = sp.course_setpoint;
-	fw_lateral_ctrl_sp.lateral_acceleration = sp.lateral_acceleration_feedforward;
-	_lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp);
-
-	const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = {
-		.timestamp = hrt_absolute_time(),
-		.altitude = pos_sp_curr.alt,
-		.height_rate = NAN,
-		.equivalent_airspeed = target_airspeed,
-		.pitch_direct = NAN,
-		.throttle_direct = NAN
-	};
-
-	_longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp);
-
-	if (pos_sp_curr.gliding_enabled) {
-		_ctrl_configuration_handler.setThrottleMin(0.0f);
-		_ctrl_configuration_handler.setThrottleMax(0.0f);
-		_ctrl_configuration_handler.setSpeedWeight(2.0f);
-	}
-}
-
 void
 FixedWingModeManager::control_auto_takeoff(const hrt_abstime &now, const float control_interval,
 		const Vector2d &global_position, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr)
@@ -2052,80 +1997,22 @@ FixedWingModeManager::Run()
 							     _local_pos.ref_timestamp);
 		}
 
-		if (_control_mode.flag_control_offboard_enabled) {
-			trajectory_setpoint_s trajectory_setpoint;
+		if (_pos_sp_triplet_sub.update(&_pos_sp_triplet)) {
 
-			if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) {
-				bool valid_setpoint = false;
-				_pos_sp_triplet = {}; // clear any existing
-				_pos_sp_triplet.timestamp = trajectory_setpoint.timestamp;
-				_pos_sp_triplet.current.timestamp = trajectory_setpoint.timestamp;
-				_pos_sp_triplet.current.cruising_speed = NAN; // ignored
-				_pos_sp_triplet.current.cruising_throttle = NAN; // ignored
-				_pos_sp_triplet.current.vx = NAN;
-				_pos_sp_triplet.current.vy = NAN;
-				_pos_sp_triplet.current.vz = NAN;
-				_pos_sp_triplet.current.lat = static_cast<double>(NAN);
-				_pos_sp_triplet.current.lon = static_cast<double>(NAN);
-				_pos_sp_triplet.current.alt = NAN;
+			_position_setpoint_previous_valid = PX4_ISFINITE(_pos_sp_triplet.previous.lat)
+							    && PX4_ISFINITE(_pos_sp_triplet.previous.lon)
+							    && PX4_ISFINITE(_pos_sp_triplet.previous.alt);
 
-				if (Vector3f(trajectory_setpoint.position).isAllFinite()) {
-					if (_global_local_proj_ref.isInitialized()) {
-						double lat;
-						double lon;
-						_global_local_proj_ref.reproject(trajectory_setpoint.position[0], trajectory_setpoint.position[1], lat, lon);
-						valid_setpoint = true;
-						_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
-						_pos_sp_triplet.current.lat = lat;
-						_pos_sp_triplet.current.lon = lon;
-						_pos_sp_triplet.current.alt = _reference_altitude - trajectory_setpoint.position[2];
-					}
+			_position_setpoint_current_valid = PX4_ISFINITE(_pos_sp_triplet.current.lat)
+							   && PX4_ISFINITE(_pos_sp_triplet.current.lon)
+							   && PX4_ISFINITE(_pos_sp_triplet.current.alt);
 
-				}
+			_position_setpoint_next_valid = PX4_ISFINITE(_pos_sp_triplet.next.lat)
+							&& PX4_ISFINITE(_pos_sp_triplet.next.lon)
+							&& PX4_ISFINITE(_pos_sp_triplet.next.alt);
 
-				if (Vector3f(trajectory_setpoint.velocity).isAllFinite()) {
-					valid_setpoint = true;
-					_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
-					_pos_sp_triplet.current.vx = trajectory_setpoint.velocity[0];
-					_pos_sp_triplet.current.vy = trajectory_setpoint.velocity[1];
-					_pos_sp_triplet.current.vz = trajectory_setpoint.velocity[2];
-
-					if (Vector3f(trajectory_setpoint.acceleration).isAllFinite()) {
-						Vector2f velocity_sp_2d(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]);
-						Vector2f normalized_velocity_sp_2d = velocity_sp_2d.normalized();
-						Vector2f acceleration_sp_2d(trajectory_setpoint.acceleration[0], trajectory_setpoint.acceleration[1]);
-						Vector2f acceleration_normal = acceleration_sp_2d - acceleration_sp_2d.dot(normalized_velocity_sp_2d) *
-									       normalized_velocity_sp_2d;
-						float direction = -normalized_velocity_sp_2d.cross(acceleration_normal.normalized());
-						_pos_sp_triplet.current.loiter_radius = direction * velocity_sp_2d.norm() * velocity_sp_2d.norm() /
-											acceleration_normal.norm();
-
-					} else {
-						_pos_sp_triplet.current.loiter_radius = NAN;
-					}
-				}
-
-				_position_setpoint_current_valid = valid_setpoint;
-			}
-
-		} else {
-			if (_pos_sp_triplet_sub.update(&_pos_sp_triplet)) {
-
-				_position_setpoint_previous_valid = PX4_ISFINITE(_pos_sp_triplet.previous.lat)
-								    && PX4_ISFINITE(_pos_sp_triplet.previous.lon)
-								    && PX4_ISFINITE(_pos_sp_triplet.previous.alt);
-
-				_position_setpoint_current_valid = PX4_ISFINITE(_pos_sp_triplet.current.lat)
-								   && PX4_ISFINITE(_pos_sp_triplet.current.lon)
-								   && PX4_ISFINITE(_pos_sp_triplet.current.alt);
-
-				_position_setpoint_next_valid = PX4_ISFINITE(_pos_sp_triplet.next.lat)
-								&& PX4_ISFINITE(_pos_sp_triplet.next.lon)
-								&& PX4_ISFINITE(_pos_sp_triplet.next.alt);
-
-				// reset the altitude foh (first order hold) logic
-				_min_current_sp_distance_xy = FLT_MAX;
-			}
+			// reset the altitude foh (first order hold) logic
+			_min_current_sp_distance_xy = FLT_MAX;
 		}
 
 		airspeed_poll();
@@ -2210,11 +2097,6 @@ FixedWingModeManager::Run()
 				break;
 			}
 
-		case FW_POSCTRL_MODE_AUTO_PATH: {
-				control_auto_path(control_interval, curr_pos, ground_speed, _pos_sp_triplet.current);
-				break;
-			}
-
 		case FW_POSCTRL_MODE_AUTO_TAKEOFF: {
 				control_auto_takeoff(now, control_interval, curr_pos, ground_speed, _pos_sp_triplet.current);
 				break;

src/modules/fw_mode_manager/FixedWingModeManager.hpp

@@ -77,7 +77,6 @@
 #include <uORB/topics/position_controller_landing_status.h>
 #include <uORB/topics/position_controller_status.h>
 #include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -181,7 +180,6 @@ private:
 	uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)};
 	uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)};
-	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
 	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
@@ -228,7 +226,6 @@ private:
 		FW_POSCTRL_MODE_AUTO_TAKEOFF_NO_NAV,
 		FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT,
 		FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR,
-		FW_POSCTRL_MODE_AUTO_PATH,
 		FW_POSCTRL_MODE_MANUAL_POSITION,
 		FW_POSCTRL_MODE_MANUAL_ALTITUDE,
 		FW_POSCTRL_MODE_TRANSITION_TO_HOVER_LINE_FOLLOW,

src/modules/uxrce_dds_client/dds_topics.yaml

@@ -160,6 +160,9 @@ subscriptions:
   - topic: /fmu/in/trajectory_setpoint
     type: px4_msgs::msg::TrajectorySetpoint
 
+  - topic: /fmu/in/global_trajectory_setpoint
+    type: px4_msgs::msg::GlobalTrajectorySetpoint
+
   - topic: /fmu/in/vehicle_attitude_setpoint
     type: px4_msgs::msg::VehicleAttitudeSetpoint