FW Position Controller rework

- split up old module into two, one handling setpoint generation, one control - add lateral and longitudinal control setpoints topics that can also be injected from companion computer - add configuration topics that (optionally) configure the controller with limits and momentary settings Signed-off-by: RomanBapst <bapstroman@gmail.com>
2026-07-08 09:30:34 +08:00 · 2025-01-08 15:34:43 +03:00
parent 52f0ef927d
commit 779a55c6dc
107 changed files with 3268 additions and 2920 deletions
@@ -0,0 +1,15 @@
+set(CONTROL_DEPENDENCIES
+        npfg
+        tecs
+)
+
+
+px4_add_module(
+        MODULE modules__fw_lateral_longitudinal_control
+        MAIN fw_lat_lon_control
+        SRCS
+        FwLateralLongitudinalControl.cpp
+        FwLateralLongitudinalControl.hpp
+        DEPENDS
+        ${CONTROL_DEPENDENCIES}
+)
@@ -0,0 +1,841 @@
+/****************************************************************************
+ *
+ * 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 "FwLateralLongitudinalControl.hpp"
+#include <px4_platform_common/events.h>
+
+using math::constrain;
+using math::max;
+using math::radians;
+
+using matrix::Dcmf;
+using matrix::Eulerf;
+using matrix::Quatf;
+using matrix::Vector2f;
+
+// [m/s] maximum reference altitude rate threshhold
+static constexpr float MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT = 0.1f;
+// [us] time after which the wind estimate is disabled if no longer updating
+static constexpr hrt_abstime WIND_EST_TIMEOUT = 10_s;
+// [s] slew rate with which we change altitude time constant
+static constexpr float TECS_ALT_TIME_CONST_SLEW_RATE = 1.0f;
+
+static constexpr float HORIZONTAL_EVH_FACTOR_COURSE_VALID{3.f}; ///< Factor of velocity standard deviation above which course calculation is considered good enough
+static constexpr float HORIZONTAL_EVH_FACTOR_COURSE_INVALID{2.f}; ///< Factor of velocity standard deviation below which course calculation is considered unsafe
+static constexpr float COS_HEADING_TRACK_ANGLE_NOT_PUSHED_BACK{0.09f}; ///< Cos of Heading to track angle below which it is assumed that the vehicle is not pushed back by the wind ~cos(85°)
+static constexpr float COS_HEADING_TRACK_ANGLE_PUSHED_BACK{0.f}; ///< Cos of Heading to track angle above which it is assumed that the vehicle is pushed back by the wind
+
+// [s] Timeout that has to pass in roll-constraining failsafe before warning is triggered
+static constexpr uint64_t ROLL_WARNING_TIMEOUT = 2_s;
+
+// [-] Can-run threshold needed to trigger the roll-constraining failsafe warning
+static constexpr float ROLL_WARNING_CAN_RUN_THRESHOLD = 0.9f;
+
+// [m/s/s] slew rate limit for airspeed setpoint changes
+static constexpr float ASPD_SP_SLEW_RATE = 1.f;
+
+FwLateralLongitudinalControl::FwLateralLongitudinalControl(bool is_vtol) :
+	ModuleParams(nullptr),
+	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
+	_attitude_sp_pub(is_vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
+	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
+{
+	_tecs_status_pub.advertise();
+	_flight_phase_estimation_pub.advertise();
+	_fixed_wing_lateral_status_pub.advertise();
+	parameters_update();
+	_airspeed_slew_rate_controller.setSlewRate(ASPD_SP_SLEW_RATE);
+}
+
+FwLateralLongitudinalControl::~FwLateralLongitudinalControl()
+{
+	perf_free(_loop_perf);
+}
+void
+FwLateralLongitudinalControl::parameters_update()
+{
+	updateParams();
+	_performance_model.updateParameters();
+	_performance_model.runSanityChecks();
+
+	_tecs.set_max_climb_rate(_performance_model.getMaximumClimbRate(_air_density));
+	_tecs.set_max_sink_rate(_param_fw_t_sink_max.get());
+	_tecs.set_min_sink_rate(_performance_model.getMinimumSinkRate(_air_density));
+	_tecs.set_equivalent_airspeed_trim(_performance_model.getCalibratedTrimAirspeed());
+	_tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed());
+	_tecs.set_equivalent_airspeed_max(_performance_model.getMaximumCalibratedAirspeed());
+	_tecs.set_throttle_damp(_param_fw_t_thr_damping.get());
+	_tecs.set_integrator_gain_throttle(_param_fw_t_thr_integ.get());
+	_tecs.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get());
+	_tecs.set_throttle_slewrate(_param_fw_thr_slew_max.get());
+	_tecs.set_vertical_accel_limit(_param_fw_t_vert_acc.get());
+	_tecs.set_roll_throttle_compensation(_param_fw_t_rll2thr.get());
+	_tecs.set_pitch_damping(_param_fw_t_ptch_damp.get());
+	_tecs.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get());
+	_tecs.set_fast_descend_altitude_error(_param_fw_t_fast_alt_err.get());
+	_tecs.set_altitude_rate_ff(_param_fw_t_hrate_ff.get());
+	_tecs.set_airspeed_error_time_constant(_param_fw_t_tas_error_tc.get());
+	_tecs.set_ste_rate_time_const(_param_ste_rate_time_const.get());
+	_tecs.set_seb_rate_ff_gain(_param_seb_rate_ff.get());
+	_tecs.set_airspeed_measurement_std_dev(_param_speed_standard_dev.get());
+	_tecs.set_airspeed_rate_measurement_std_dev(_param_speed_rate_standard_dev.get());
+	_tecs.set_airspeed_filter_process_std_dev(_param_process_noise_standard_dev.get());
+
+	_roll_slew_rate.setSlewRate(radians(_param_fw_pn_r_slew_max.get()));
+
+	_tecs_alt_time_const_slew_rate.setSlewRate(TECS_ALT_TIME_CONST_SLEW_RATE);
+	_tecs_alt_time_const_slew_rate.setForcedValue(_param_fw_t_h_error_tc.get() * _param_fw_thrtc_sc.get());
+}
+
+void FwLateralLongitudinalControl::Run()
+{
+	if (should_exit()) {
+		_local_pos_sub.unregisterCallback();
+		exit_and_cleanup();
+		return;
+	}
+
+	perf_begin(_loop_perf);
+
+	/* only run controller if position changed */
+	// 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();
+	}
+
+	if (_local_pos_sub.update(&_local_pos)) {
+
+		const float control_interval = math::constrain((_local_pos.timestamp - _last_time_loop_ran) * 1e-6f,
+					       0.001f, 0.1f);
+		_last_time_loop_ran = _local_pos.timestamp;
+
+		updateControllerConfiguration();
+
+		_tecs.set_speed_weight(_long_configuration.speed_weight);
+		updateTECSAltitudeTimeConstant(checkLowHeightConditions()
+					       || _long_configuration.enforce_low_height_condition, control_interval);
+		_tecs.set_altitude_error_time_constant(_tecs_alt_time_const_slew_rate.getState());
+
+		if (_vehicle_air_data_sub.updated()) {
+			_vehicle_air_data_sub.update();
+			_air_density = PX4_ISFINITE(_vehicle_air_data_sub.get().rho) ? _vehicle_air_data_sub.get().rho : _air_density;
+			_tecs.set_max_climb_rate(_performance_model.getMaximumClimbRate(_air_density));
+			_tecs.set_min_sink_rate(_performance_model.getMinimumSinkRate(_air_density));
+		}
+
+		if (_vehicle_landed_sub.updated()) {
+			vehicle_land_detected_s landed{};
+			_vehicle_landed_sub.copy(&landed);
+			_landed = landed.landed;
+		}
+
+		_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN;
+
+		_vehicle_status_sub.update();
+		_control_mode_sub.update();
+		update_control_state();
+
+		if (_control_mode_sub.get().flag_control_manual_enabled && _control_mode_sub.get().flag_control_altitude_enabled
+		    && _local_pos.z_reset_counter != _z_reset_counter) {
+			if (_control_mode_sub.get().flag_control_altitude_enabled && _local_pos.z_reset_counter != _z_reset_counter) {
+				// make TECS accept step in altitude and demanded altitude
+				_tecs.handle_alt_step(_long_control_state.altitude_msl, _long_control_state.height_rate);
+			}
+		}
+
+		const bool should_run = (_control_mode_sub.get().flag_control_position_enabled ||
+					 _control_mode_sub.get().flag_control_velocity_enabled ||
+					 _control_mode_sub.get().flag_control_acceleration_enabled ||
+					 _control_mode_sub.get().flag_control_altitude_enabled ||
+					 _control_mode_sub.get().flag_control_climb_rate_enabled) &&
+					(_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
+					 || _vehicle_status_sub.get().in_transition_mode);
+
+		if (should_run) {
+
+			// ----- Longitudinal ------
+			float pitch_sp{NAN};
+			float throttle_sp{NAN};
+
+			if (_fw_longitudinal_ctrl_sub.updated()) {
+				_fw_longitudinal_ctrl_sub.copy(&_long_control_sp);
+			}
+
+			const float airspeed_sp_eas = adapt_airspeed_setpoint(control_interval, _long_control_sp.equivalent_airspeed,
+						      _min_airspeed_from_guidance, _lateral_control_state.wind_speed.length());
+
+			// If the both altitude and height rate are set, set altitude setpoint to NAN
+			const float altitude_sp = PX4_ISFINITE(_long_control_sp.height_rate) ? NAN : _long_control_sp.altitude;
+
+			tecs_update_pitch_throttle(control_interval, altitude_sp,
+						   airspeed_sp_eas,
+						   _long_configuration.pitch_min,
+						   _long_configuration.pitch_max,
+						   _long_configuration.throttle_min,
+						   _long_configuration.throttle_max,
+						   _long_configuration.sink_rate_target,
+						   _long_configuration.climb_rate_target,
+						   _long_configuration.disable_underspeed_protection,
+						   _long_control_sp.height_rate
+						  );
+
+			pitch_sp = PX4_ISFINITE(_long_control_sp.pitch_direct) ? _long_control_sp.pitch_direct : _tecs.get_pitch_setpoint();
+			throttle_sp = PX4_ISFINITE(_long_control_sp.throttle_direct) ? _long_control_sp.throttle_direct :
+				      _tecs.get_throttle_setpoint();
+
+			// ----- Lateral ------
+			float roll_sp {NAN};
+
+			if (_fw_lateral_ctrl_sub.updated()) {
+				// We store the update of _fw_lateral_ctrl_sub in a member variable instead of only local such that we can run
+				// the controllers also without new setpoints.
+				_fw_lateral_ctrl_sub.copy(&_lat_control_sp);
+			}
+
+			float airspeed_direction_sp{NAN};
+			float lateral_accel_sp {NAN};
+			const Vector2f airspeed_vector = _lateral_control_state.ground_speed - _lateral_control_state.wind_speed;
+
+			if (PX4_ISFINITE(_lat_control_sp.course) && !PX4_ISFINITE(_lat_control_sp.airspeed_direction)) {
+				// only use the course setpoint if it's finite but airspeed_direction is not
+
+				airspeed_direction_sp = _course_to_airspeed.mapCourseSetpointToHeadingSetpoint(
+								_lat_control_sp.course, _lateral_control_state.wind_speed,
+								airspeed_sp_eas);
+
+				// Note: the here updated _min_airspeed_from_guidance is only used in the next iteration
+				// in the longitudinal controller.
+				const float max_true_airspeed = _performance_model.getMaximumCalibratedAirspeed() * _long_control_state.eas2tas;
+				_min_airspeed_from_guidance = _course_to_airspeed.getMinAirspeedForCurrentBearing(
+								      _lat_control_sp.course, _lateral_control_state.wind_speed,
+								      max_true_airspeed, _param_fw_gnd_spd_min.get())
+							      / _long_control_state.eas2tas;
+
+			} else if (PX4_ISFINITE(_lat_control_sp.airspeed_direction)) {
+				// If the airspeed_direction is finite we use that instead of the course.
+
+				airspeed_direction_sp = _lat_control_sp.airspeed_direction;
+				_min_airspeed_from_guidance = 0.f; // reset if no longer in course control
+
+			} else {
+				_min_airspeed_from_guidance = 0.f; // reset if no longer in course control
+			}
+
+			if (PX4_ISFINITE(airspeed_direction_sp)) {
+				const float heading = atan2f(airspeed_vector(1), airspeed_vector(0));
+				lateral_accel_sp = _airspeed_direction_control.controlHeading(airspeed_direction_sp, heading,
+						   airspeed_vector.norm());
+			}
+
+			if (PX4_ISFINITE(_lat_control_sp.lateral_acceleration)) {
+				lateral_accel_sp = PX4_ISFINITE(lateral_accel_sp) ? lateral_accel_sp + _lat_control_sp.lateral_acceleration :
+						   _lat_control_sp.lateral_acceleration;
+			}
+
+			if (!PX4_ISFINITE(lateral_accel_sp)) {
+				lateral_accel_sp = 0.f; // mitigation if no valid setpoint is received: 0 lateral acceleration
+			}
+
+			lateral_accel_sp = getCorrectedLateralAccelSetpoint(lateral_accel_sp);
+			lateral_accel_sp = math::constrain(lateral_accel_sp, -_lateral_configuration.lateral_accel_max,
+							   _lateral_configuration.lateral_accel_max);
+			roll_sp = mapLateralAccelerationToRollAngle(lateral_accel_sp);
+
+			fixed_wing_lateral_status_s fixed_wing_lateral_status{};
+			fixed_wing_lateral_status.timestamp = hrt_absolute_time();
+			fixed_wing_lateral_status.lateral_acceleration = lateral_accel_sp;
+			fixed_wing_lateral_status.can_run_factor = _can_run_factor;
+
+			_fixed_wing_lateral_status_pub.publish(fixed_wing_lateral_status);
+
+			// additional is_finite checks that should not be necessary, but are kept for safety
+			float roll_body = PX4_ISFINITE(roll_sp) ? roll_sp : 0.0f;
+			float pitch_body = PX4_ISFINITE(pitch_sp) ? pitch_sp : 0.0f;
+			const float yaw_body = _yaw; // yaw is not controlled in fixed wing, need to set it though for quaternion generation
+			const float thrust_body_x = PX4_ISFINITE(throttle_sp) ? throttle_sp : 0.0f;
+
+			if (_control_mode_sub.get().flag_control_manual_enabled) {
+				roll_body = constrain(roll_body, -radians(_param_fw_r_lim.get()),
+						      radians(_param_fw_r_lim.get()));
+				pitch_body = constrain(pitch_body, radians(_param_fw_p_lim_min.get()),
+						       radians(_param_fw_p_lim_max.get()));
+			}
+
+			// roll slew rate
+			roll_body = _roll_slew_rate.update(roll_body, control_interval);
+
+			_att_sp.timestamp = hrt_absolute_time();
+			const Quatf q(Eulerf(roll_body, pitch_body, yaw_body));
+			q.copyTo(_att_sp.q_d);
+
+			_att_sp.thrust_body[0] = thrust_body_x;
+
+			_attitude_sp_pub.publish(_att_sp);
+
+		}
+
+		_z_reset_counter = _local_pos.z_reset_counter;
+	}
+
+	perf_end(_loop_perf);
+}
+
+void FwLateralLongitudinalControl::updateControllerConfiguration()
+{
+	if (_lateral_configuration.timestamp == 0) {
+		_lateral_configuration.timestamp = _local_pos.timestamp;
+		_lateral_configuration.lateral_accel_max = tanf(radians(_param_fw_r_lim.get())) * CONSTANTS_ONE_G;
+
+	}
+
+	if (_long_configuration.timestamp == 0) {
+		setDefaultLongitudinalControlConfiguration();
+	}
+
+	if (_long_control_configuration_sub.updated() || _parameter_update_sub.updated()) {
+		longitudinal_control_configuration_s configuration_in{};
+		_long_control_configuration_sub.copy(&configuration_in);
+		updateLongitudinalControlConfiguration(configuration_in);
+	}
+
+	if (_lateral_control_configuration_sub.updated() || _parameter_update_sub.updated()) {
+		lateral_control_configuration_s configuration_in{};
+		_lateral_control_configuration_sub.copy(&configuration_in);
+		_lateral_configuration.timestamp = configuration_in.timestamp;
+
+		if (PX4_ISFINITE(configuration_in.lateral_accel_max)) {
+			_lateral_configuration.lateral_accel_max = min(configuration_in.lateral_accel_max, tanf(radians(
+						_param_fw_r_lim.get())) * CONSTANTS_ONE_G);
+
+		} else {
+			_lateral_configuration.lateral_accel_max = tanf(radians(_param_fw_r_lim.get())) * CONSTANTS_ONE_G;
+		}
+	}
+}
+
+void
+FwLateralLongitudinalControl::tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
+		float pitch_min_rad, float pitch_max_rad, float throttle_min,
+		float throttle_max, const float desired_max_sinkrate,
+		const float desired_max_climbrate,
+		bool disable_underspeed_detection, float hgt_rate_sp)
+{
+	bool tecs_is_running = true;
+
+	// do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition
+	if (_vehicle_status_sub.get().is_vtol
+	    && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
+		|| _vehicle_status_sub.get().in_transition_mode)) {
+		tecs_is_running = false;
+		return;
+
+	}
+
+	const float throttle_trim_compensated = _performance_model.getTrimThrottle(throttle_min,
+						throttle_max, airspeed_sp, _air_density);
+
+	_tecs.set_detect_underspeed_enabled(!disable_underspeed_detection);
+
+	// HOTFIX: the airspeed rate estimate using acceleration in body-forward direction has shown to lead to high biases
+	// when flying tight turns. It's in this case much safer to just set the estimated airspeed rate to 0.
+	const float airspeed_rate_estimate = 0.f;
+
+	_tecs.update(_long_control_state.pitch_rad - radians(_param_fw_psp_off.get()),
+		     _long_control_state.altitude_msl,
+		     alt_sp,
+		     airspeed_sp,
+		     _long_control_state.airspeed_eas,
+		     _long_control_state.eas2tas,
+		     throttle_min,
+		     throttle_max,
+		     throttle_trim_compensated,
+		     pitch_min_rad - radians(_param_fw_psp_off.get()),
+		     pitch_max_rad - radians(_param_fw_psp_off.get()),
+		     desired_max_climbrate,
+		     desired_max_sinkrate,
+		     airspeed_rate_estimate,
+		     _long_control_state.height_rate,
+		     hgt_rate_sp);
+
+	tecs_status_publish(alt_sp, airspeed_sp, airspeed_rate_estimate, throttle_trim_compensated);
+
+	if (tecs_is_running && !_vehicle_status_sub.get().in_transition_mode
+	    && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) {
+		const TECS::DebugOutput &tecs_output{_tecs.getStatus()};
+
+		// Check level flight: the height rate setpoint is not set or set to 0 and we are close to the target altitude and target altitude is not moving
+		if ((fabsf(tecs_output.height_rate_reference) < MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT) &&
+		    fabsf(_long_control_state.altitude_msl - tecs_output.altitude_reference) < _param_nav_fw_alt_rad.get()) {
+			_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_LEVEL;
+
+		} else if (((tecs_output.altitude_reference - _long_control_state.altitude_msl) >= _param_nav_fw_alt_rad.get()) ||
+			   (tecs_output.height_rate_reference >= MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT)) {
+			_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_CLIMB;
+
+		} else if (((_long_control_state.altitude_msl - tecs_output.altitude_reference) >= _param_nav_fw_alt_rad.get()) ||
+			   (tecs_output.height_rate_reference <= -MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT)) {
+			_flight_phase_estimation_pub.get().flight_phase = flight_phase_estimation_s::FLIGHT_PHASE_DESCEND;
+
+		} else {
+			//We can't infer the flight phase , do nothing, estimation is reset at each step
+		}
+	}
+}
+
+void
+FwLateralLongitudinalControl::tecs_status_publish(float alt_sp, float equivalent_airspeed_sp,
+		float true_airspeed_derivative_raw, float throttle_trim)
+{
+	tecs_status_s tecs_status{};
+
+	const TECS::DebugOutput &debug_output{_tecs.getStatus()};
+
+	tecs_status.altitude_sp = alt_sp;
+	tecs_status.altitude_reference = debug_output.altitude_reference;
+	tecs_status.altitude_time_constant = _tecs.get_altitude_error_time_constant();
+	tecs_status.height_rate_reference = debug_output.height_rate_reference;
+	tecs_status.height_rate_direct = debug_output.height_rate_direct;
+	tecs_status.height_rate_setpoint = debug_output.control.altitude_rate_control;
+	tecs_status.height_rate = -_local_pos.vz;
+	tecs_status.equivalent_airspeed_sp = equivalent_airspeed_sp;
+	tecs_status.true_airspeed_sp = debug_output.true_airspeed_sp;
+	tecs_status.true_airspeed_filtered = debug_output.true_airspeed_filtered;
+	tecs_status.true_airspeed_derivative_sp = debug_output.control.true_airspeed_derivative_control;
+	tecs_status.true_airspeed_derivative = debug_output.true_airspeed_derivative;
+	tecs_status.true_airspeed_derivative_raw = true_airspeed_derivative_raw;
+	tecs_status.total_energy_rate = debug_output.control.total_energy_rate_estimate;
+	tecs_status.total_energy_balance_rate = debug_output.control.energy_balance_rate_estimate;
+	tecs_status.total_energy_rate_sp = debug_output.control.total_energy_rate_sp;
+	tecs_status.total_energy_balance_rate_sp = debug_output.control.energy_balance_rate_sp;
+	tecs_status.throttle_integ = debug_output.control.throttle_integrator;
+	tecs_status.pitch_integ = debug_output.control.pitch_integrator;
+	tecs_status.throttle_sp = _tecs.get_throttle_setpoint();
+	tecs_status.pitch_sp_rad = _tecs.get_pitch_setpoint();
+	tecs_status.throttle_trim = throttle_trim;
+	tecs_status.underspeed_ratio = _tecs.get_underspeed_ratio();
+	tecs_status.fast_descend_ratio = debug_output.fast_descend;
+
+	tecs_status.timestamp = hrt_absolute_time();
+
+	_tecs_status_pub.publish(tecs_status);
+}
+
+int FwLateralLongitudinalControl::task_spawn(int argc, char *argv[])
+{
+	bool is_vtol = false;
+
+	if (argc > 1) {
+		if (strcmp(argv[1], "vtol") == 0) {
+			is_vtol = true;
+		}
+	}
+
+	FwLateralLongitudinalControl *instance = new FwLateralLongitudinalControl(is_vtol);
+
+	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;
+}
+
+bool
+FwLateralLongitudinalControl::init()
+{
+	if (!_local_pos_sub.registerCallback()) {
+		PX4_ERR("callback registration failed");
+		return false;
+	}
+
+	return true;
+}
+
+int FwLateralLongitudinalControl::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int FwLateralLongitudinalControl::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+fw_lat_lon_control computes attitude and throttle setpoints from lateral and longitudinal control setpoints.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("fw_lat_lon_control", "controller");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+void FwLateralLongitudinalControl::update_control_state() {
+	updateAltitudeAndHeightRate();
+	updateAirspeed();
+	updateAttitude();
+	updateWind();
+
+	_lateral_control_state.ground_speed = Vector2f(_local_pos.vx, _local_pos.vy);
+}
+
+void FwLateralLongitudinalControl::updateWind() {
+	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 = hrt_absolute_time();
+
+		_lateral_control_state.wind_speed(0) = wind.windspeed_north;
+		_lateral_control_state.wind_speed(1) = wind.windspeed_east;
+
+	} else {
+		// invalidate wind estimate usage (and correspondingly NPFG, if enabled) after subscription timeout
+		_wind_valid = _wind_valid && (hrt_absolute_time() - _time_wind_last_received) < WIND_EST_TIMEOUT;
+	}
+
+	if (!_wind_valid) {
+		_lateral_control_state.wind_speed.setZero();
+	}
+}
+
+void FwLateralLongitudinalControl::updateAltitudeAndHeightRate() {
+	float ref_alt{0.f};
+	if (_local_pos.z_global && PX4_ISFINITE(_local_pos.ref_alt)) {
+		ref_alt = _local_pos.ref_alt;
+	}
+
+	_long_control_state.altitude_msl = -_local_pos.z + ref_alt; // Altitude AMSL in meters
+	_long_control_state.height_rate = -_local_pos.vz;
+
+}
+
+void FwLateralLongitudinalControl::updateAttitude() {
+	vehicle_attitude_s att;
+
+	if (_vehicle_attitude_sub.update(&att)) {
+
+		Dcmf R{Quatf(att.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_sub.get().is_vtol_tailsitter) {
+			const Dcmf R_offset{Eulerf{0.f, M_PI_2_F, 0.f}};
+			R = R * R_offset;
+		}
+
+		const Eulerf euler_angles(R);
+		_long_control_state.pitch_rad = euler_angles.theta();
+		_yaw = euler_angles.psi();
+
+		// load factor due to banking
+		const float load_factor_from_bank_angle = 1.0f / max(cosf(euler_angles.phi()), FLT_EPSILON);
+		_tecs.set_load_factor(load_factor_from_bank_angle);
+	}
+}
+
+void FwLateralLongitudinalControl::updateAirspeed() {
+
+	airspeed_validated_s airspeed_validated;
+
+	if (_param_fw_use_airspd.get() && _airspeed_validated_sub.update(&airspeed_validated)) {
+
+		// do not use synthetic airspeed as this would create a thrust loop
+		if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)
+		    && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)
+		    && airspeed_validated.airspeed_source != airspeed_validated_s::SYNTHETIC) {
+
+			_time_airspeed_last_valid = airspeed_validated.timestamp;
+			_long_control_state.airspeed_eas = airspeed_validated.calibrated_airspeed_m_s;
+			_long_control_state.eas2tas = constrain(airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s, 0.9f, 2.0f);
+		}
+	}
+
+	// no airspeed updates for one second --> declare invalid
+	const bool airspeed_valid = hrt_elapsed_time(&_time_airspeed_last_valid) < 1_s;
+
+	if (!airspeed_valid) {
+		_long_control_state.eas2tas = 1.f;
+	}
+
+	_tecs.enable_airspeed(airspeed_valid);
+}
+
+float
+FwLateralLongitudinalControl::adapt_airspeed_setpoint(const float control_interval, float calibrated_airspeed_setpoint,
+		float calibrated_min_airspeed_guidance, float wind_speed)
+{
+	float system_min_airspeed = _performance_model.getMinimumCalibratedAirspeed(getLoadFactor());
+
+	const float system_max_airspeed = _performance_model.getMaximumCalibratedAirspeed();
+
+	// airspeed setpoint adjustments
+	if (!PX4_ISFINITE(calibrated_airspeed_setpoint) || calibrated_airspeed_setpoint <= FLT_EPSILON) {
+		calibrated_airspeed_setpoint = _performance_model.getCalibratedTrimAirspeed();
+
+		// Aditional option to increase the min airspeed setpoint based on wind estimate for more stability in higher winds.
+		if (_wind_valid && _param_fw_wind_arsp_sc.get() > FLT_EPSILON) {
+			system_min_airspeed = math::min(system_min_airspeed + _param_fw_wind_arsp_sc.get() *
+			wind_speed, system_max_airspeed);
+		}
+	}
+
+	// increase setpoint to at what's at least required for the lateral guidance
+	calibrated_airspeed_setpoint = math::max(calibrated_airspeed_setpoint, calibrated_min_airspeed_guidance);
+
+	// constrain airspeed to feasible range
+	calibrated_airspeed_setpoint = math::constrain(calibrated_airspeed_setpoint, system_min_airspeed, system_max_airspeed);
+
+	if (!PX4_ISFINITE(_airspeed_slew_rate_controller.getState())) {
+
+		// initialize the airspeed setpoint
+		if (PX4_ISFINITE(_long_control_state.airspeed_eas) && _long_control_state.airspeed_eas < system_min_airspeed) {
+			// current airpseed is below minimum - init with minimum
+			_airspeed_slew_rate_controller.setForcedValue(system_min_airspeed);
+
+		} else if (PX4_ISFINITE(_long_control_state.airspeed_eas) && _long_control_state.airspeed_eas > system_max_airspeed) {
+			// current airpseed is above maximum - init with maximum
+			_airspeed_slew_rate_controller.setForcedValue(system_max_airspeed);
+
+		} else if (PX4_ISFINITE(_long_control_state.airspeed_eas)) {
+			// current airpseed is between min and max - init with current
+			_airspeed_slew_rate_controller.setForcedValue(_long_control_state.airspeed_eas);
+
+		} else {
+			// current airpseed is invalid - init with setpoint
+			_airspeed_slew_rate_controller.setForcedValue(calibrated_airspeed_setpoint);
+		}
+	} else {
+		// update slew rate state
+		if (_airspeed_slew_rate_controller.getState() < system_min_airspeed) {
+			// current airpseed setpoint is below minimum - reset to minimum
+			_airspeed_slew_rate_controller.setForcedValue(system_min_airspeed);
+
+		} else if (_airspeed_slew_rate_controller.getState() > system_max_airspeed) {
+			// current airpseed setpoint is above maximum - reset to maximum
+			_airspeed_slew_rate_controller.setForcedValue(system_max_airspeed);
+
+		} else if (PX4_ISFINITE(_long_control_state.airspeed_eas)) {
+			// current airpseed setpoint is between min and max - update
+			_airspeed_slew_rate_controller.update(calibrated_airspeed_setpoint, control_interval);
+
+		}
+	}
+
+	return _airspeed_slew_rate_controller.getState();
+}
+
+bool FwLateralLongitudinalControl::checkLowHeightConditions() const
+{
+	// Are conditions for low-height
+	return _param_fw_t_thr_low_hgt.get() >= 0.f && _local_pos.dist_bottom_valid
+	       && _local_pos.dist_bottom < _param_fw_t_thr_low_hgt.get();
+}
+
+void FwLateralLongitudinalControl::updateTECSAltitudeTimeConstant(const bool is_low_height, const float dt)
+{
+	// Target time constant for the TECS altitude tracker
+	float alt_tracking_tc = _param_fw_t_h_error_tc.get();
+
+	if (is_low_height) {
+		// If low-height conditions satisfied, compute target time constant for altitude tracking
+		alt_tracking_tc *= _param_fw_thrtc_sc.get();
+	}
+
+	_tecs_alt_time_const_slew_rate.update(alt_tracking_tc, dt);
+}
+
+float FwLateralLongitudinalControl::getGuidanceQualityFactor(const vehicle_local_position_s &local_pos, const bool is_wind_valid) const
+{
+	if (is_wind_valid) {
+		// If we have a valid wind estimate, npfg is able to handle all degenerated cases
+		return 1.f;
+	}
+
+	// NPFG can run without wind information as long as the system is not flying backwards and has a minimal ground speed
+	// Check the minimal ground speed. if it is greater than twice the standard deviation, we assume that we can infer a valid track angle
+	const Vector2f ground_vel(local_pos.vx, local_pos.vy);
+	const float ground_speed(ground_vel.norm());
+	const float low_ground_speed_factor(math::constrain((ground_speed - HORIZONTAL_EVH_FACTOR_COURSE_INVALID *
+									    local_pos.evh) / ((HORIZONTAL_EVH_FACTOR_COURSE_VALID - HORIZONTAL_EVH_FACTOR_COURSE_INVALID)*local_pos.evh),
+							    0.f, 1.f));
+
+	// Check that the angle between heading and track is not off too much. if it is greater than 90° we will be pushed back from the wind and the npfg will propably give a roll command in the wrong direction.
+	const Vector2f heading_vector(matrix::Dcm2f(local_pos.heading)*Vector2f({1.f, 0.f}));
+	const Vector2f ground_vel_norm(ground_vel.normalized());
+	const float flying_forward_factor(math::constrain((heading_vector.dot(ground_vel_norm) -
+							   COS_HEADING_TRACK_ANGLE_PUSHED_BACK) / ((COS_HEADING_TRACK_ANGLE_NOT_PUSHED_BACK -
+												    COS_HEADING_TRACK_ANGLE_PUSHED_BACK)),
+							  0.f, 1.f));
+
+	return flying_forward_factor * low_ground_speed_factor;
+}
+float FwLateralLongitudinalControl::getCorrectedLateralAccelSetpoint(float lateral_accel_sp)
+{
+	// Scale the npfg output to zero if npfg is not certain for correct output
+	_can_run_factor = math::constrain(getGuidanceQualityFactor(_local_pos, _wind_valid), 0.f, 1.f);
+
+	hrt_abstime now{hrt_absolute_time()};
+
+	// Warn the user when the scale is less than 90% for at least 2 seconds (disable in transition)
+
+	// If the npfg was not running before, reset the user warning variables.
+	if ((now - _time_since_last_npfg_call) > ROLL_WARNING_TIMEOUT) {
+		_need_report_npfg_uncertain_condition = true;
+		_time_since_first_reduced_roll = 0U;
+	}
+
+	if (_vehicle_status_sub.get().in_transition_mode || _can_run_factor > ROLL_WARNING_CAN_RUN_THRESHOLD || _landed) {
+		// NPFG reports a good condition or we are in transition, reset the user warning variables.
+		_need_report_npfg_uncertain_condition = true;
+		_time_since_first_reduced_roll = 0U;
+
+	} else if (_need_report_npfg_uncertain_condition) {
+		if (_time_since_first_reduced_roll == 0U) {
+			_time_since_first_reduced_roll = now;
+		}
+
+		if ((now - _time_since_first_reduced_roll) > ROLL_WARNING_TIMEOUT) {
+			_need_report_npfg_uncertain_condition = false;
+			events::send(events::ID("npfg_roll_command_uncertain"), events::Log::Warning,
+				     "Roll command reduced due to uncertain velocity/wind estimates!");
+		}
+
+	} else {
+		// Nothing to do, already reported.
+	}
+
+	_time_since_last_npfg_call = now;
+
+	return _can_run_factor * (lateral_accel_sp);
+}
+float FwLateralLongitudinalControl::mapLateralAccelerationToRollAngle(float lateral_acceleration_sp) const {
+	return  atanf(lateral_acceleration_sp / CONSTANTS_ONE_G);
+}
+
+void FwLateralLongitudinalControl::setDefaultLongitudinalControlConfiguration() {
+	_long_configuration.timestamp = hrt_absolute_time();
+	_long_configuration.pitch_min = radians(_param_fw_p_lim_min.get());
+	_long_configuration.pitch_max = radians(_param_fw_p_lim_max.get());
+	_long_configuration.throttle_min = _param_fw_thr_min.get();
+	_long_configuration.throttle_max = _param_fw_thr_max.get();
+	_long_configuration.climb_rate_target = _param_climbrate_target.get();
+	_long_configuration.sink_rate_target = _param_sinkrate_target.get();
+	_long_configuration.disable_underspeed_protection = false;
+	_long_configuration.enforce_low_height_condition = false;
+}
+
+void FwLateralLongitudinalControl::updateLongitudinalControlConfiguration(const longitudinal_control_configuration_s &configuration_in) {
+	_long_configuration.timestamp = configuration_in.timestamp;
+
+	if (PX4_ISFINITE(configuration_in.pitch_min)) {
+		_long_configuration.pitch_min = math::constrain(configuration_in.pitch_min, radians(_param_fw_p_lim_min.get()), radians(_param_fw_p_lim_max.get()));
+	} else {
+		_long_configuration.pitch_min = radians(_param_fw_p_lim_min.get());
+	}
+
+	if (PX4_ISFINITE(configuration_in.pitch_max)) {
+		_long_configuration.pitch_max = math::constrain(configuration_in.pitch_max, _long_configuration.pitch_min, radians(_param_fw_p_lim_max.get()));
+	} else {
+		_long_configuration.pitch_max = radians(_param_fw_p_lim_max.get());
+	}
+
+	if (PX4_ISFINITE(configuration_in.throttle_min)) {
+		_long_configuration.throttle_min = math::constrain(configuration_in.throttle_min, _param_fw_thr_min.get(), _param_fw_thr_max.get());
+	} else {
+		_long_configuration.throttle_min = _param_fw_thr_min.get();
+	}
+
+	if (PX4_ISFINITE(configuration_in.throttle_max)) {
+		_long_configuration.throttle_max = math::constrain(configuration_in.throttle_max, _long_configuration.throttle_min, _param_fw_thr_max.get());
+	} else {
+		_long_configuration.throttle_max = _param_fw_thr_max.get();
+	}
+
+	if (PX4_ISFINITE(configuration_in.climb_rate_target)) {
+		_long_configuration.climb_rate_target = math::max(0.0f, configuration_in.climb_rate_target);
+	} else {
+		_long_configuration.climb_rate_target = _param_climbrate_target.get();
+	}
+
+	if (PX4_ISFINITE(configuration_in.sink_rate_target)) {
+		_long_configuration.sink_rate_target = math::max(0.0f, configuration_in.sink_rate_target);
+	} else {
+		_long_configuration.sink_rate_target = _param_sinkrate_target.get();
+	}
+}
+
+float FwLateralLongitudinalControl::getLoadFactor() const
+{
+	float load_factor_from_bank_angle = 1.f;
+
+	const float roll_body = Eulerf(Quatf(_att_sp.q_d)).phi();
+
+	if (PX4_ISFINITE(roll_body)) {
+		load_factor_from_bank_angle = 1.f / math::max(cosf(roll_body), FLT_EPSILON);
+	}
+
+	return load_factor_from_bank_angle;
+}
+
+extern "C" __EXPORT int fw_lat_lon_control_main(int argc, char *argv[])
+{
+	return FwLateralLongitudinalControl::main(argc, argv);
+}
@@ -0,0 +1,260 @@
+/****************************************************************************
+ *
+ *   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 FwLateralLongitudinalControl.hpp
+ */
+
+#ifndef PX4_FWLATERALLONGITUDINALCONTROL_HPP
+#define PX4_FWLATERALLONGITUDINALCONTROL_HPP
+
+#include <float.h>
+#include <fw_performance_model/PerformanceModel.hpp>
+#include <drivers/drv_hrt.h>
+#include <lib/geo/geo.h>
+#include <lib/atmosphere/atmosphere.h>
+#include <lib/npfg/CourseToAirspeedRefMapper.hpp>
+#include <lib/npfg/AirspeedDirectionController.hpp>
+#include <lib/tecs/TECS.hpp>
+#include <lib/mathlib/mathlib.h>
+#include <lib/perf/perf_counter.h>
+#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 <slew_rate/SlewRate.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_status.h>
+#include <uORB/topics/fixed_wing_longitudinal_setpoint.h>
+#include <uORB/topics/flight_phase_estimation.h>
+#include <uORB/topics/lateral_control_configuration.h>
+#include <uORB/topics/longitudinal_control_configuration.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/tecs_status.h>
+#include <uORB/topics/vehicle_air_data.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/wind.h>
+
+static constexpr fixed_wing_lateral_setpoint_s empty_lateral_control_setpoint = {.timestamp = 0, .course = NAN, .airspeed_direction = NAN, .lateral_acceleration = NAN};
+static constexpr 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};
+
+class FwLateralLongitudinalControl final : public ModuleBase<FwLateralLongitudinalControl>, public ModuleParams,
+	public px4::WorkItem
+{
+public:
+	FwLateralLongitudinalControl(bool is_vtol);
+
+	~FwLateralLongitudinalControl() 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::SubscriptionData<vehicle_control_mode_s> _control_mode_sub{ORB_ID(vehicle_control_mode)};
+	uORB::SubscriptionData<vehicle_air_data_s> _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _vehicle_landed_sub{ORB_ID(vehicle_land_detected)};
+	uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _fw_lateral_ctrl_sub{ORB_ID(fixed_wing_lateral_setpoint)};
+	uORB::Subscription _fw_longitudinal_ctrl_sub{ORB_ID(fixed_wing_longitudinal_setpoint)};
+	uORB::Subscription _long_control_configuration_sub{ORB_ID(longitudinal_control_configuration)};
+	uORB::Subscription _lateral_control_configuration_sub{ORB_ID(lateral_control_configuration)};
+
+	vehicle_local_position_s _local_pos{};
+	fixed_wing_longitudinal_setpoint_s _long_control_sp{empty_longitudinal_control_setpoint};
+	longitudinal_control_configuration_s _long_configuration{};
+	fixed_wing_lateral_setpoint_s _lat_control_sp{empty_lateral_control_setpoint};
+	lateral_control_configuration_s _lateral_configuration{};
+
+	uORB::Publication <vehicle_attitude_setpoint_s> _attitude_sp_pub;
+	uORB::Publication <tecs_status_s> _tecs_status_pub{ORB_ID(tecs_status)};
+	uORB::PublicationData <flight_phase_estimation_s> _flight_phase_estimation_pub{ORB_ID(flight_phase_estimation)};
+	uORB::Publication <fixed_wing_lateral_status_s> _fixed_wing_lateral_status_pub{ORB_ID(fixed_wing_lateral_status)};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off,
+		(ParamBool<px4::params::FW_USE_AIRSPD>) _param_fw_use_airspd,
+		(ParamFloat<px4::params::NAV_FW_ALT_RAD>) _param_nav_fw_alt_rad,
+		(ParamFloat<px4::params::FW_R_LIM>) _param_fw_r_lim,
+		(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_PN_R_SLEW_MAX>) _param_fw_pn_r_slew_max,
+		(ParamFloat<px4::params::FW_T_HRATE_FF>) _param_fw_t_hrate_ff,
+		(ParamFloat<px4::params::FW_T_ALT_TC>) _param_fw_t_h_error_tc,
+		(ParamFloat<px4::params::FW_T_F_ALT_ERR>) _param_fw_t_fast_alt_err,
+		(ParamFloat<px4::params::FW_T_THR_INTEG>) _param_fw_t_thr_integ,
+		(ParamFloat<px4::params::FW_T_I_GAIN_PIT>) _param_fw_t_I_gain_pit,
+		(ParamFloat<px4::params::FW_T_PTCH_DAMP>) _param_fw_t_ptch_damp,
+		(ParamFloat<px4::params::FW_T_RLL2THR>) _param_fw_t_rll2thr,
+		(ParamFloat<px4::params::FW_T_SINK_MAX>) _param_fw_t_sink_max,
+		(ParamFloat<px4::params::FW_T_TAS_TC>) _param_fw_t_tas_error_tc,
+		(ParamFloat<px4::params::FW_T_THR_DAMPING>) _param_fw_t_thr_damping,
+		(ParamFloat<px4::params::FW_T_VERT_ACC>) _param_fw_t_vert_acc,
+		(ParamFloat<px4::params::FW_T_STE_R_TC>) _param_ste_rate_time_const,
+		(ParamFloat<px4::params::FW_T_SEB_R_FF>) _param_seb_rate_ff,
+		(ParamFloat<px4::params::FW_T_SPD_STD>) _param_speed_standard_dev,
+		(ParamFloat<px4::params::FW_T_SPD_DEV_STD>) _param_speed_rate_standard_dev,
+		(ParamFloat<px4::params::FW_T_SPD_PRC_STD>) _param_process_noise_standard_dev,
+		(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_MAX>) _param_fw_thr_max,
+		(ParamFloat<px4::params::FW_THR_MIN>) _param_fw_thr_min,
+		(ParamFloat<px4::params::FW_THR_SLEW_MAX>) _param_fw_thr_slew_max,
+		(ParamFloat<px4::params::FW_LND_THRTC_SC>) _param_fw_thrtc_sc,
+		(ParamFloat<px4::params::FW_T_THR_LOW_HGT>) _param_fw_t_thr_low_hgt,
+		(ParamFloat<px4::params::FW_WIND_ARSP_SC>) _param_fw_wind_arsp_sc,
+		(ParamFloat<px4::params::FW_GND_SPD_MIN>) _param_fw_gnd_spd_min
+	)
+
+	hrt_abstime _last_time_loop_ran{};
+	uint8_t _z_reset_counter{UINT8_C(0)};
+	uint64_t _time_airspeed_last_valid{UINT64_C(0)};
+	float _air_density{atmosphere::kAirDensitySeaLevelStandardAtmos};
+	// Smooths changes in the altitude tracking error time constant value
+	SlewRate<float> _tecs_alt_time_const_slew_rate;
+	struct longitudinal_control_state {
+		float pitch_rad{0.f};
+		float altitude_msl{0.f};
+		float airspeed_eas{0.f};
+		float eas2tas{1.f};
+		float height_rate{0.f};
+	} _long_control_state{};
+
+	bool _wind_valid{false};
+	hrt_abstime _time_wind_last_received{0};
+	SlewRate<float> _roll_slew_rate;
+	float _yaw{0.f};
+	struct lateral_control_state {
+		matrix::Vector2f ground_speed;
+		matrix::Vector2f wind_speed;
+	} _lateral_control_state{};
+	bool _need_report_npfg_uncertain_condition{false}; ///< boolean if reporting of uncertain npfg output condition is needed
+	hrt_abstime _time_since_first_reduced_roll{0U}; ///< absolute time since start when entering reduced roll angle for the first time
+	hrt_abstime _time_since_last_npfg_call{0U}; 	///< absolute time since start when the npfg reduced roll angle calculations was last performed
+	vehicle_attitude_setpoint_s _att_sp{};
+	bool _landed{false};
+	float _can_run_factor{0.f};
+	SlewRate<float> _airspeed_slew_rate_controller;
+
+	perf_counter_t _loop_perf; // loop performance counter
+
+	PerformanceModel _performance_model;
+	TECS _tecs;
+	CourseToAirspeedRefMapper _course_to_airspeed;
+	AirspeedDirectionController _airspeed_direction_control;
+
+	float _min_airspeed_from_guidance{0.f}; // need to store it bc we only update after running longitudinal controller
+
+	void parameters_update();
+	void update_control_state();
+	void tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
+					float pitch_min_rad, float pitch_max_rad, float throttle_min,
+					float throttle_max, const float desired_max_sinkrate,
+					const float desired_max_climbrate,
+					bool disable_underspeed_detection, float hgt_rate_sp);
+
+	void tecs_status_publish(float alt_sp, float equivalent_airspeed_sp, float true_airspeed_derivative_raw,
+				 float throttle_trim);
+
+	void updateAirspeed();
+
+	void updateAttitude();
+
+	void updateAltitudeAndHeightRate();
+
+	float mapLateralAccelerationToRollAngle(float lateral_acceleration_sp) const;
+
+	void updateWind();
+
+	void updateTECSAltitudeTimeConstant(const bool is_low_height, const float dt);
+
+	bool checkLowHeightConditions() const;
+
+	float getGuidanceQualityFactor(const vehicle_local_position_s &local_pos, const bool is_wind_valid) const;
+
+	float getCorrectedLateralAccelSetpoint(float lateral_accel_sp);
+
+	void setDefaultLongitudinalControlConfiguration();
+
+	void updateLongitudinalControlConfiguration(const longitudinal_control_configuration_s &configuration_in);
+
+	void updateControllerConfiguration();
+
+	float getLoadFactor() const;
+
+	/**
+	 * @brief Returns an adapted calibrated airspeed setpoint
+	 *
+	 * Adjusts the setpoint for wind, accelerated stall, and slew rates.
+	 *
+	 * @param control_interval Time since the last position control update [s]
+	 * @param calibrated_airspeed_setpoint Calibrated airspeed septoint (generally from the position setpoint) [m/s]
+	 * @param calibrated_min_airspeed_guidance Minimum airspeed required for lateral guidance [m/s]
+	 * @return Adjusted calibrated airspeed setpoint [m/s]
+	 */
+	float adapt_airspeed_setpoint(const float control_interval, float calibrated_airspeed_setpoint,
+				      float calibrated_min_airspeed_guidance, float wind_speed);
+};
+
+#endif //PX4_FWLATERALLONGITUDINALCONTROL_HPP
@@ -0,0 +1,5 @@
+menuconfig MODULES_FW_LATERAL_LONGITUDINAL_CONTROL
+	bool "fw_lateral_longitudinal_control"
+	default n
+	---help---
+		Enable support for fw_lat_lon_control
@@ -0,0 +1,284 @@
+/**
+ * Path navigation roll slew rate limit.
+ *
+ * Maximum change in roll angle setpoint per second.
+ * Applied in all Auto modes, plus manual Position & Altitude modes.
+ *
+ * @unit deg/s
+ * @min 0
+ * @decimal 0
+ * @increment 1
+ * @group FW Path Control
+ */
+PARAM_DEFINE_FLOAT(FW_PN_R_SLEW_MAX, 90.0f);
+
+/**
+ * Minimum groundspeed
+ *
+ * The controller will increase the commanded airspeed to maintain
+ * this minimum groundspeed to the next waypoint.
+ *
+ * @unit m/s
+ * @min 0.0
+ * @max 40
+ * @decimal 1
+ * @increment 0.5
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_GND_SPD_MIN, 5.0f);
+
+
+
+
+// ----------longitudinal params----------
+
+/**
+ * Throttle max slew rate
+ *
+ * Maximum slew rate for the commanded throttle
+ *
+ * @min 0.0
+ * @max 1.0
+ * @decimal 2
+ * @increment 0.01
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_THR_SLEW_MAX, 0.0f);
+
+/**
+ * Low-height threshold for tighter altitude tracking
+ *
+ * Height above ground threshold below which tighter altitude
+ * tracking gets enabled (see FW_LND_THRTC_SC). Below this height, TECS smoothly
+ * (1 sec / sec) transitions the altitude tracking time constant from FW_T_ALT_TC
+ * to FW_LND_THRTC_SC*FW_T_ALT_TC.
+ *
+ * -1 to disable.
+ *
+ * @unit m
+ * @min -1
+ * @decimal 0
+ * @increment 1
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_THR_LOW_HGT, -1.f);
+
+/**
+ * Throttle damping factor
+ *
+ * This is the damping gain for the throttle demand loop.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @decimal 3
+ * @increment 0.01
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_THR_DAMPING, 0.05f);
+
+/**
+ * Integrator gain throttle
+ *
+ * Increase it to trim out speed and height offsets faster,
+ * with the downside of possible overshoots and oscillations.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @decimal 3
+ * @increment 0.005
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_THR_INTEG, 0.02f);
+
+/**
+ * Integrator gain pitch
+ *
+ * Increase it to trim out speed and height offsets faster,
+ * with the downside of possible overshoots and oscillations.
+ *
+ * @min 0.0
+ * @max 2.0
+ * @decimal 2
+ * @increment 0.05
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_I_GAIN_PIT, 0.1f);
+
+/**
+ * Maximum vertical acceleration
+ *
+ * This is the maximum vertical acceleration
+ * either up or down that the controller will use to correct speed
+ * or height errors.
+ *
+ * @unit m/s^2
+ * @min 1.0
+ * @max 10.0
+ * @decimal 1
+ * @increment 0.5
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
+
+/**
+ * Airspeed measurement standard deviation
+ *
+ * For the airspeed filter in TECS.
+ *
+ * @unit m/s
+ * @min 0.01
+ * @max 10.0
+ * @decimal 2
+ * @increment 0.1
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_SPD_STD, 0.07f);
+
+/**
+ * Airspeed rate measurement standard deviation
+ *
+ * For the airspeed filter in TECS.
+ *
+ * @unit m/s^2
+ * @min 0.01
+ * @max 10.0
+ * @decimal 2
+ * @increment 0.1
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_SPD_DEV_STD, 0.2f);
+
+/**
+ * Process noise standard deviation for the airspeed rate
+ *
+ * This is defining the noise in the airspeed rate for the constant airspeed rate model
+ * of the TECS airspeed filter.
+ *
+ * @unit m/s^2
+ * @min 0.01
+ * @max 10.0
+ * @decimal 2
+ * @increment 0.1
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_SPD_PRC_STD, 0.2f);
+
+/**
+ * Roll -> Throttle feedforward
+ *
+ * Is used to compensate for the additional drag created by turning.
+ * Increase this gain if the aircraft initially loses energy in turns
+ * and reduce if the aircraft initially gains energy in turns.
+ *
+ * @min 0.0
+ * @max 20.0
+ * @decimal 1
+ * @increment 0.5
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 15.0f);
+
+/**
+ * Pitch damping gain
+ *
+ * @min 0.0
+ * @max 2.0
+ * @decimal 2
+ * @increment 0.1
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.1f);
+
+/**
+ * Altitude error time constant.
+ *
+ * @min 2.0
+ * @decimal 2
+ * @increment 0.5
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_ALT_TC, 5.0f);
+
+/**
+ * Fast descend: minimum altitude error
+ *
+ * Minimum altitude error needed to descend with max airspeed and minimal throttle.
+ * A negative value disables fast descend.
+ *
+ * @min -1.0
+ * @decimal 0
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_F_ALT_ERR, -1.0f);
+
+/**
+ * Height rate feed forward
+ *
+ * @min 0.0
+ * @max 1.0
+ * @decimal 2
+ * @increment 0.05
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.3f);
+
+/**
+ * True airspeed error time constant.
+ *
+ * @min 2.0
+ * @decimal 2
+ * @increment 0.5
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_TAS_TC, 5.0f);
+
+/**
+ * Specific total energy rate first order filter time constant.
+ *
+ * This filter is applied to the specific total energy rate used for throttle damping.
+ *
+ * @min 0.0
+ * @max 2
+ * @decimal 2
+ * @increment 0.01
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_STE_R_TC, 0.4f);
+
+/**
+ * Specific total energy balance rate feedforward gain.
+ *
+ *
+ * @min 0.5
+ * @max 3
+ * @decimal 2
+ * @increment 0.01
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_SEB_R_FF, 1.0f);
+
+/**
+ * Wind-based airspeed scaling factor
+ *
+ * Multiplying this factor with the current absolute wind estimate gives the airspeed offset
+ * added to the minimum airspeed setpoint limit. This helps to make the
+ * system more robust against disturbances (turbulence) in high wind.
+ *
+ * @min 0
+ * @decimal 2
+ * @increment 0.01
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_WIND_ARSP_SC, 0.f);
+
+/**
+ * Maximum descent rate
+ *
+ * @unit m/s
+ * @min 1.0
+ * @max 15.0
+ * @decimal 1
+ * @increment 0.5
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
@@ -60,6 +60,8 @@ px4_add_module(
 	SRCS
 		FixedwingPositionControl.cpp
 		FixedwingPositionControl.hpp
+		ControllerConfigurationHandler.cpp
+		ControllerConfigurationHandler.hpp
 	DEPENDS
 		${POSCONTROL_DEPENDENCIES}
 	)
@@ -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;
+}
@@ -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
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013-2022 PX4 Development Team. All rights reserved.
+ *   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
@@ -35,13 +35,6 @@
 /**
 * @file FixedwingPositionControl.hpp
 * Implementation of various fixed-wing position level navigation/control modes.
- *
- * The implementation for the controllers is in a separate library. This class only
- * interfaces to the library.
- *
- * @author Lorenz Meier <lorenz@px4.io>
- * @author Thomas Gubler <thomasgubler@gmail.com>
- * @author Andreas Antener <andreas@uaventure.com>
 */

 #ifndef FIXEDWINGPOSITIONCONTROL_HPP_
@@ -49,15 +42,13 @@

 #include "launchdetection/LaunchDetector.h"
 #include "runway_takeoff/RunwayTakeoff.h"
-#include <lib/fw_performance_model/PerformanceModel.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/npfg.hpp>
-#include <lib/tecs/TECS.hpp>
+#include <lib/npfg/DirectionalGuidance.hpp>
 #include <lib/mathlib/mathlib.h>
 #include <lib/perf/perf_counter.h>
 #include <lib/slew_rate/SlewRate.hpp>
@@ -67,24 +58,25 @@
 #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/flight_phase_estimation.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/landing_gear.h>
 #include <uORB/topics/launch_detection_status.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/normalized_unsigned_setpoint.h>
-#include <uORB/topics/npfg_status.h>
 #include <uORB/topics/parameter_update.h>
 #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/tecs_status.h>
 #include <uORB/topics/trajectory_setpoint.h>
-#include <uORB/topics/vehicle_air_data.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -97,12 +89,10 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/wind.h>
 #include <uORB/topics/orbit_status.h>
-#include <uORB/uORB.h>

 #ifdef CONFIG_FIGURE_OF_EIGHT
 #include "figure_eight/FigureEight.hpp"
 #include <uORB/topics/figure_eight_status.h>
-
 #endif // CONFIG_FIGURE_OF_EIGHT

 using namespace launchdetection;
@@ -115,12 +105,6 @@ 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;

-// [m] distance (plane to waypoint in front) at which waypoints are reset in heading hold mode
-static constexpr float HDG_HOLD_REACHED_DIST = 1000.0f;
-
-// [m] distance by which previous waypoint is set behind the plane
-static constexpr float HDG_HOLD_SET_BACK_DIST = 100.0f;
-
 // [rad/s] max yawrate at which plane locks yaw for heading hold mode
 static constexpr float HDG_HOLD_YAWRATE_THRESH = 0.15f;

@@ -138,9 +122,6 @@ 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;

-// [m/s/s] slew rate limit for airspeed setpoint changes
-static constexpr float ASPD_SP_SLEW_RATE = 1.f;
-
 // [us] time after which the wind estimate is disabled if no longer updating
 static constexpr hrt_abstime WIND_EST_TIMEOUT = 10_s;

@@ -166,23 +147,14 @@ 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;

-// [m/s] maximum reference altitude rate threshhold
-static constexpr float MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT = 0.1f;
-
-// [s] Timeout that has to pass in roll-constraining failsafe before warning is triggered
-static constexpr uint64_t ROLL_WARNING_TIMEOUT = 2_s;
-
-// [-] Can-run threshold needed to trigger the roll-constraining failsafe warning
-static constexpr float ROLL_WARNING_CAN_RUN_THRESHOLD = 0.9f;
-
-// [s] slew rate with which we change altitude time constant
-static constexpr float TECS_ALT_TIME_CONST_SLEW_RATE = 1.0f;
+// [] Stick deadzon
+static constexpr float kStickDeadBand = 0.06f;

 class FixedwingPositionControl final : public ModuleBase<FixedwingPositionControl>, public ModuleParams,
 	public px4::WorkItem
 {
 public:
-	FixedwingPositionControl(bool vtol = false);
+	FixedwingPositionControl();
 	~FixedwingPositionControl() override;

 	/** @see ModuleBase */
@@ -210,33 +182,31 @@ private:
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)};
 	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
-	uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
 	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_attitude_setpoint_s> _attitude_sp_pub;
 	uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)};
-	uORB::Publication<npfg_status_s> _npfg_status_pub{ORB_ID(npfg_status)};
-	uORB::Publication<position_controller_status_s>	_pos_ctrl_status_pub{ORB_ID(position_controller_status)};
 	uORB::Publication<position_controller_landing_status_s>	_pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)};
-	uORB::Publication<tecs_status_s> _tecs_status_pub{ORB_ID(tecs_status)};
 	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::Publication<landing_gear_s> _landing_gear_pub {ORB_ID(landing_gear)};
 	uORB::Publication<normalized_unsigned_setpoint_s> _flaps_setpoint_pub{ORB_ID(flaps_setpoint)};
 	uORB::Publication<normalized_unsigned_setpoint_s> _spoilers_setpoint_pub{ORB_ID(spoilers_setpoint)};
-	uORB::PublicationData<flight_phase_estimation_s> _flight_phase_estimation_pub{ORB_ID(flight_phase_estimation)};
+	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)};

 	manual_control_setpoint_s _manual_control_setpoint{};
 	position_setpoint_triplet_s _pos_sp_triplet{};
-	vehicle_attitude_setpoint_s _att_sp{};
 	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};
@@ -272,12 +242,12 @@ private:

 	// VEHICLE STATES

+	uint8_t _nav_state;
+
 	double _current_latitude{0};
 	double _current_longitude{0};
 	float _current_altitude{0.f};

-	float _roll{0.f};
-	float _pitch{0.0f};
 	float _yaw{0.0f};
 	float _yawrate{0.0f};

@@ -323,8 +293,8 @@ private:
 	// 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)
-	Vector2d _launch_global_position{0, 0};
+	// [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};
@@ -363,7 +333,6 @@ private:
 		bool flaring{false};
 		hrt_abstime start_time{0}; // [us]
 		float initial_height_rate_setpoint{0.0f}; // [m/s]
-		float initial_throttle_setpoint{0.0f};
 	} _flare_states;

 	// [m] last terrain estimate which was valid
@@ -381,9 +350,7 @@ private:
 	// AIRSPEED

 	float _airspeed_eas{0.f};
-	float _eas2tas{1.f};
 	bool _airspeed_valid{false};
-	float _air_density{atmosphere::kAirDensitySeaLevelStandardAtmos};

 	// [us] last time airspeed was received. used to detect timeouts.
 	hrt_abstime _time_airspeed_last_valid{0};
@@ -397,24 +364,12 @@ private:

 	hrt_abstime _time_wind_last_received{0}; // [us]

-	// TECS
-
-	// total energy control system - airspeed / altitude control
-	TECS _tecs;
-
-	bool _tecs_is_running{false};
-
-	// Smooths changes in the altitude tracking error time constant value
-	SlewRate<float> _tecs_alt_time_const_slew_rate;
-
 	// VTOL / TRANSITION
-	bool _is_vtol_tailsitter{false};
 	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};
-	uint8_t _z_reset_counter{0};
 	uint64_t _time_last_xy_reset{0};

 	// LATERAL-DIRECTIONAL GUIDANCE
@@ -423,12 +378,7 @@ private:
 	matrix::Vector2f _closest_point_on_path;

 	// nonlinear path following guidance - lateral-directional position control
-	NPFG _npfg;
-	bool _need_report_npfg_uncertain_condition{false}; ///< boolean if reporting of uncertain npfg output condition is needed
-	hrt_abstime _time_since_first_reduced_roll{0U}; ///< absolute time since start when entering reduced roll angle for the first time
-	hrt_abstime _time_since_last_npfg_call{0U}; 	///< absolute time since start when the npfg reduced roll angle calculations was last performed
-
-	PerformanceModel _performance_model;
+	DirectionalGuidance _directional_guidance;

 	// LANDING GEAR
 	int8_t _new_landing_gear_position{landing_gear_s::GEAR_KEEP};
@@ -439,7 +389,6 @@ private:

 	hrt_abstime _time_in_fixed_bank_loiter{0}; // [us]
 	float _min_current_sp_distance_xy{FLT_MAX};
-	float _target_bearing{0.0f}; // [rad]

 #ifdef CONFIG_FIGURE_OF_EIGHT
 	/* Loitering */
@@ -451,11 +400,10 @@ private:
 	 * @param control_interval Time since last position control call [s]
 	 * @param curr_pos the current 2D absolute position of the vehicle in [deg].
 	 * @param ground_speed the 2D ground speed of the vehicle in [m/s].
-	 * @param pos_sp_prev the previous position setpoint.
 	 * @param pos_sp_curr the current position setpoint.
 	 */
 	void controlAutoFigureEight(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
-				    const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
+				    const position_setpoint_s &pos_sp_curr);

 	void publishFigureEightStatus(const position_setpoint_s pos_sp);
 #endif // CONFIG_FIGURE_OF_EIGHT
@@ -465,27 +413,18 @@ private:

 	// Update subscriptions
 	void airspeed_poll();
-	void control_update();
+
 	void manual_control_setpoint_poll();
 	void vehicle_attitude_poll();
 	void vehicle_command_poll();
 	void vehicle_control_mode_poll();
-	void vehicle_status_poll();
-	void wind_poll();

-	void status_publish();
+	void wind_poll(const hrt_abstime now);
+
 	void landing_status_publish();
-	void tecs_status_publish(float alt_sp, float equivalent_airspeed_sp, float true_airspeed_derivative_raw,
-				 float throttle_trim);
-	void publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint);
-	float getLoadFactor();

-	/**
-	 * @brief Get the NPFG roll setpoint with mitigation strategy if npfg is not certain about its output
-	 *
-	 * @return roll setpoint
-	 */
-	float getCorrectedNpfgRollSetpoint();
+	void publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint);
+	float getLoadFactor() const;

 	/**
 	 * @brief Sets the landing abort status and publishes landing status.
@@ -503,24 +442,6 @@ private:
 	 */
 	bool checkLandingAbortBitMask(const uint8_t automatic_abort_criteria_bitmask, uint8_t landing_abort_criterion);

-	/**
-	 * @brief Get a new waypoint based on heading and distance from current position
-	 *
-	 * @param heading the heading to fly to
-	 * @param distance the distance of the generated waypoint
-	 * @param waypoint_prev the waypoint at the current position
-	 * @param waypoint_next the waypoint in the heading direction
-	 */
-	void get_waypoint_heading_distance(float heading, position_setpoint_s &waypoint_prev,
-					   position_setpoint_s &waypoint_next, bool flag_init);
-
-	/**
-	 * @brief Return the terrain estimate during takeoff or takeoff_alt if terrain estimate is not available
-	 *
-	 * @param takeoff_alt Altitude AMSL at launch or when runway takeoff is detected [m]
-	 */
-	float get_terrain_altitude_takeoff(float takeoff_alt);
-
 	/**
 	 * @brief Maps the manual control setpoint (pilot sticks) to height rate commands
 	 *
@@ -535,13 +456,6 @@ private:
 	 */
 	void updateManualTakeoffStatus();

-	/**
-	 * @brief Update desired altitude base on user pitch stick input
-	 *
-	 * @param dt Time step
-	 */
-	void update_desired_altitude(float dt);
-
 	/**
 	 * @brief Updates timing information for landed and in-air states.
 	 *
@@ -585,19 +499,15 @@ private:
 	 * @brief Controls altitude and airspeed for a fixed-bank loiter.
 	 *
 	 * Used as a failsafe mode after a lateral position estimate failure.
-	 *
-	 * @param control_interval Time since last position control call [s]
 	 */
-	void control_auto_fixed_bank_alt_hold(const float control_interval);
+	void control_auto_fixed_bank_alt_hold();

 	/**
 	 * @brief Control airspeed with a fixed descent rate and roll angle.
 	 *
 	 * Used as a failsafe mode after a lateral position estimate failure.
-	 *
-	 * @param control_interval Time since last position control call [s]
 	 */
-	void control_auto_descend(const float control_interval);
+	void control_auto_descend();

 	/**
 	 * @brief Vehicle control for position waypoints.
@@ -617,12 +527,11 @@ private:
 	 * @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
 	 * @param pos_sp_next next position setpoint
 	 */
 	void control_auto_loiter(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
-				 const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
+				 const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);


 	/**
@@ -703,11 +612,13 @@ private:
 	/**
 	 * @brief Controls user commanded altitude, airspeed, and bearing.
 	 *
+	 * @param now Current system time [us]
 	 * @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]
 	 */
-	void control_manual_position(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed);
+	void control_manual_position(const hrt_abstime now, const float control_interval, const Vector2d &curr_pos,
+				     const Vector2f &ground_speed);

 	/**
 	 * @brief Holds the initial heading during the course of a transition to hover. Used when there is no local
@@ -724,26 +635,8 @@ private:
 	void control_backtransition_line_follow(const Vector2f &ground_speed,
 						const position_setpoint_s &pos_sp_curr);

-	float get_tecs_pitch();
-	float get_tecs_thrust();
-
 	float get_manual_airspeed_setpoint();

-	/**
-	 * @brief Returns an adapted calibrated airspeed setpoint
-	 *
-	 * Adjusts the setpoint for wind, accelerated stall, and slew rates.
-	 *
-	 * @param control_interval Time since the last position control update [s]
-	 * @param calibrated_airspeed_setpoint Calibrated airspeed septoint (generally from the position setpoint) [m/s]
-	 * @param calibrated_min_airspeed Minimum calibrated airspeed [m/s]
-	 * @param ground_speed Vehicle ground velocity vector (NE) [m/s]
-	 * @param in_takeoff_situation Vehicle is currently in a takeoff situation
-	 * @return Adjusted calibrated airspeed setpoint [m/s]
-	 */
-	float adapt_airspeed_setpoint(const float control_interval, float calibrated_airspeed_setpoint,
-				      float calibrated_min_airspeed, const Vector2f &ground_speed, bool in_takeoff_situation = false);
-
 	void reset_takeoff_state();
 	void reset_landing_state();

@@ -758,39 +651,7 @@ private:

 	void publishOrbitStatus(const position_setpoint_s pos_sp);

-	SlewRate<float> _airspeed_slew_rate_controller;
-	SlewRate<float> _roll_slew_rate;
-
-	/**
-	 * @brief A wrapper function to call the TECS implementation
-	 *
-	 * @param control_interval Time since the last position control update [s]
-	 * @param alt_sp Altitude setpoint, AMSL [m]
-	 * @param airspeed_sp Calibrated airspeed setpoint [m/s]
-	 * @param pitch_min_rad Nominal pitch angle command minimum [rad]
-	 * @param pitch_max_rad Nominal pitch angle command maximum [rad]
-	 * @param throttle_min Minimum throttle command [0,1]
-	 * @param throttle_max Maximum throttle command [0,1]
-	 * @param desired_max_sink_rate The desired max sink rate commandable when altitude errors are large [m/s]
-	 * @param desired_max_climb_rate The desired max climb rate commandable when altitude errors are large [m/s]
-	 * @param is_low_height Define whether we are in low-height flight for tighter altitude tracking
-	 * @param disable_underspeed_detection True if underspeed detection should be disabled
-	 * @param hgt_rate_sp Height rate setpoint [m/s]
-	 */
-	void tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp, float pitch_min_rad,
-					float pitch_max_rad, float throttle_min, float throttle_max,
-					const float desired_max_sink_rate, const float desired_max_climb_rate, const bool is_low_height,
-					bool disable_underspeed_detection = false, float hgt_rate_sp = NAN);
-
-	/**
-	 * @brief Constrains the roll angle setpoint near ground to avoid wingtip strike.
-	 *
-	 * @param roll_setpoint Unconstrained roll angle setpoint [rad]
-	 * @param altitude Vehicle altitude (AMSL) [m]
-	 * @param terrain_altitude Terrain altitude (AMSL) [m]
-	 * @return Constrained roll angle setpoint [rad]
-	 */
-	float constrainRollNearGround(const float roll_setpoint, const float altitude, const float terrain_altitude) const;
+	float getMaxRollAngleNearGround(const float altitude, const float terrain_altitude) const;

 	/**
 	 * @brief Calculates the touchdown position for landing with optional manual lateral adjustments.
@@ -838,21 +699,6 @@ private:
 	void initializeAutoLanding(const hrt_abstime &now, const position_setpoint_s &pos_sp_prev,
 				   const float land_point_alt, const Vector2f &local_position, const Vector2f &local_land_point);

-	/*
-	 * Checks if the vehicle satisfies conditions for low-height flight
-	 *
-	 * @return bool True if conditions are satisfied, false otherwise
-	 */
-	bool checkLowHeightConditions();
-
-	/*
-	 * Updates TECS altitude time constant according to the is_low_height parameter.
-	 *
-	 * @param is_low_height Boolean flag defining whether we are in low-height flight
-	 * @param dt Update time step [s]
-	 */
-	void updateTECSAltitudeTimeConstant(const bool is_low_height, const float dt);
-
 	/*
 	 * Waypoint handling logic following closely to the ECL_L1_Pos_Controller
 	 * method of the same name. Takes two waypoints, steering the vehicle to track
@@ -864,9 +710,10 @@ private:
 	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
 	 * @param[in] wind_vel Wind velocity vector [m/s]
 	 */
-	void navigateWaypoints(const matrix::Vector2f &start_waypoint, const matrix::Vector2f &end_waypoint,
-			       const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &ground_vel,
-			       const matrix::Vector2f &wind_vel);
+	DirectionalGuidanceOutput navigateWaypoints(const matrix::Vector2f &start_waypoint,
+			const matrix::Vector2f &end_waypoint,
+			const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &ground_vel,
+			const matrix::Vector2f &wind_vel);

 	/*
 	 * Takes one waypoint and steers the vehicle towards this.
@@ -879,8 +726,8 @@ private:
 	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
 	 * @param[in] wind_vel Wind velocity vector [m/s]
 	 */
-	void navigateWaypoint(const matrix::Vector2f &waypoint_pos, const matrix::Vector2f &vehicle_pos,
-			      const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel);
+	DirectionalGuidanceOutput navigateWaypoint(const matrix::Vector2f &waypoint_pos, const matrix::Vector2f &vehicle_pos,
+			const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel);

 	/*
 	 * Line (infinite) following logic. Two points on the line are used to define the
@@ -893,8 +740,9 @@ private:
 	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
 	 * @param[in] wind_vel Wind velocity vector [m/s]
 	 */
-	void navigateLine(const Vector2f &point_on_line_1, const Vector2f &point_on_line_2, const Vector2f &vehicle_pos,
-			  const Vector2f &ground_vel, const Vector2f &wind_vel);
+	DirectionalGuidanceOutput navigateLine(const Vector2f &point_on_line_1, const Vector2f &point_on_line_2,
+					       const Vector2f &vehicle_pos,
+					       const Vector2f &ground_vel, const Vector2f &wind_vel);

 	/*
 	 * Line (infinite) following logic. One point on the line and a line bearing are used to define
@@ -907,8 +755,9 @@ private:
 	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
 	 * @param[in] wind_vel Wind velocity vector [m/s]
 	 */
-	void navigateLine(const Vector2f &point_on_line, const float line_bearing, const Vector2f &vehicle_pos,
-			  const Vector2f &ground_vel, const Vector2f &wind_vel);
+	DirectionalGuidanceOutput navigateLine(const Vector2f &point_on_line, const float line_bearing,
+					       const Vector2f &vehicle_pos,
+					       const Vector2f &ground_vel, const Vector2f &wind_vel);

 	/*
 	 * Loitering (unlimited) logic. Takes loiter center, radius, and direction and
@@ -922,9 +771,9 @@ private:
 	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
 	 * @param[in] wind_vel Wind velocity vector [m/s]
 	 */
-	void navigateLoiter(const matrix::Vector2f &loiter_center, const matrix::Vector2f &vehicle_pos,
-			    float radius, bool loiter_direction_counter_clockwise, const matrix::Vector2f &ground_vel,
-			    const matrix::Vector2f &wind_vel);
+	DirectionalGuidanceOutput navigateLoiter(const matrix::Vector2f &loiter_center, const matrix::Vector2f &vehicle_pos,
+			float radius, bool loiter_direction_counter_clockwise, const matrix::Vector2f &ground_vel,
+			const matrix::Vector2f &wind_vel);

 	/*
 	 * Path following logic. Takes poisiton, path tangent, curvature and
@@ -939,9 +788,10 @@ private:
 	 * @param[in] wind_vel Wind velocity vector [m/s]
 	 * @param[in] curvature of the path setpoint [1/m]
 	 */
-	void 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);
+	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);

 	/*
 	 * Navigate on a fixed bearing.
@@ -954,23 +804,22 @@ private:
 	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
 	 * @param[in] wind_vel Wind velocity vector [m/s]
 	 */
-	void navigateBearing(const matrix::Vector2f &vehicle_pos, float bearing, const matrix::Vector2f &ground_vel,
-			     const matrix::Vector2f &wind_vel);
+	DirectionalGuidanceOutput navigateBearing(const matrix::Vector2f &vehicle_pos, float bearing,
+			const matrix::Vector2f &ground_vel,
+			const matrix::Vector2f &wind_vel);
+
+	void control_idle();
+	void publish_lateral_guidance_status(const hrt_abstime now);
+
+	float rollAngleToLateralAccel(float roll_body) const;

 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::FW_GND_SPD_MIN>) _param_fw_gnd_spd_min,
-
-		(ParamFloat<px4::params::FW_PN_R_SLEW_MAX>) _param_fw_pn_r_slew_max,
 		(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,
-		(ParamBool<px4::params::NPFG_TRACK_KEEP>) _param_npfg_en_track_keeping,
-		(ParamBool<px4::params::NPFG_EN_MIN_GSP>) _param_npfg_en_min_gsp,
-		(ParamBool<px4::params::NPFG_WIND_REG>) _param_npfg_en_wind_reg,
-		(ParamFloat<px4::params::NPFG_GSP_MAX_TK>) _param_npfg_track_keeping_gsp_max,
 		(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,
@@ -980,80 +829,46 @@ private:
 		(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,
-		(ParamFloat<px4::params::FW_LND_THRTC_SC>) _param_fw_thrtc_sc,
-		(ParamFloat<px4::params::FW_T_THR_LOW_HGT>) _param_fw_t_thr_low_hgt,
 		(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_HRATE_FF>) _param_fw_t_hrate_ff,
-		(ParamFloat<px4::params::FW_T_ALT_TC>) _param_fw_t_h_error_tc,
-		(ParamFloat<px4::params::FW_T_F_ALT_ERR>) _param_fw_t_fast_alt_err,
-		(ParamFloat<px4::params::FW_T_THR_INTEG>) _param_fw_t_thr_integ,
-		(ParamFloat<px4::params::FW_T_I_GAIN_PIT>) _param_fw_t_I_gain_pit,
-		(ParamFloat<px4::params::FW_T_PTCH_DAMP>) _param_fw_t_ptch_damp,
-		(ParamFloat<px4::params::FW_T_RLL2THR>) _param_fw_t_rll2thr,
-		(ParamFloat<px4::params::FW_T_SINK_MAX>) _param_fw_t_sink_max,
-		(ParamFloat<px4::params::FW_T_SPDWEIGHT>) _param_fw_t_spdweight,
-		(ParamFloat<px4::params::FW_T_TAS_TC>) _param_fw_t_tas_error_tc,
-		(ParamFloat<px4::params::FW_T_THR_DAMPING>) _param_fw_t_thr_damping,
-		(ParamFloat<px4::params::FW_T_VERT_ACC>) _param_fw_t_vert_acc,
-		(ParamFloat<px4::params::FW_T_STE_R_TC>) _param_ste_rate_time_const,
-		(ParamFloat<px4::params::FW_T_SEB_R_FF>) _param_seb_rate_ff,
 		(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_T_SPD_STD>) _param_speed_standard_dev,
-		(ParamFloat<px4::params::FW_T_SPD_DEV_STD>) _param_speed_rate_standard_dev,
-		(ParamFloat<px4::params::FW_T_SPD_PRC_STD>) _param_process_noise_standard_dev,
-
 		(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_THR_SLEW_MAX>) _param_fw_thr_slew_max,
-
 		(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::FW_PSP_OFF>) _param_fw_psp_off,
-
 		(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,
-
-		(ParamFloat<px4::params::RWTO_NPFG_PERIOD>) _param_rwto_npfg_period,
 		(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_WIND_ARSP_SC>) _param_fw_wind_arsp_sc,
-
 		(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
+		(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 // FIXEDWINGPOSITIONCONTROL_HPP_
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022-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
@@ -48,11 +48,10 @@ static constexpr bool SOUTH_CIRCLE_IS_COUNTER_CLOCKWISE{true};
 static constexpr float DEFAULT_MAJOR_TO_MINOR_AXIS_RATIO{2.5f};
 static constexpr float MINIMAL_FEASIBLE_MAJOR_TO_MINOR_AXIS_RATIO{2.0f};

-FigureEight::FigureEight(NPFG &npfg, matrix::Vector2f &wind_vel, float &eas2tas) :
+FigureEight::FigureEight(DirectionalGuidance &npfg, matrix::Vector2f &wind_vel) :
 	ModuleParams(nullptr),
-	_npfg(npfg),
-	_wind_vel(wind_vel),
-	_eas2tas(eas2tas)
+	_directional_guidance(npfg),
+	_wind_vel(wind_vel)
 {

 }
@@ -64,8 +63,9 @@ void FigureEight::resetPattern()
 	_pos_passed_circle_center_along_major_axis = false;
 }

-void FigureEight::updateSetpoint(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
-				 const FigureEightPatternParameters &parameters, float target_airspeed)
+DirectionalGuidanceOutput FigureEight::updateSetpoint(const matrix::Vector2f &curr_pos_local,
+		const matrix::Vector2f &ground_speed,
+		const FigureEightPatternParameters &parameters)
 {
 	// Sanitize inputs
 	FigureEightPatternParameters valid_parameters{sanitizeParameters(parameters)};
@@ -81,7 +81,7 @@ void FigureEight::updateSetpoint(const matrix::Vector2f &curr_pos_local, const m
 	updateSegment(curr_pos_local, valid_parameters,  pattern_points);

 	// Apply control logic based on segment
-	applyControl(curr_pos_local, ground_speed, valid_parameters, target_airspeed, pattern_points);
+	return applyControl(curr_pos_local, ground_speed, valid_parameters, pattern_points);
 }

 FigureEight::FigureEightPatternParameters FigureEight::sanitizeParameters(const FigureEightPatternParameters
@@ -118,7 +118,8 @@ void FigureEight::initializePattern(const matrix::Vector2f &curr_pos_local, cons
 		const bool north_is_closer = north_center_to_pos_local.norm() < south_center_to_pos_local.norm();

 		// Get the normalized switch distance.
-		float switch_distance_normalized = _npfg.switchDistance(FLT_MAX) * NORMALIZED_MAJOR_RADIUS / parameters.loiter_radius;
+		float switch_distance_normalized = _directional_guidance.switchDistance(FLT_MAX) * NORMALIZED_MAJOR_RADIUS /
+						   parameters.loiter_radius;

 		//Far away from current figure of eight. Fly towards closer circle

@@ -194,7 +195,8 @@ void FigureEight::updateSegment(const matrix::Vector2f &curr_pos_local, const Fi
 	calculatePositionToCenterNormalizedRotated(center_to_pos_local, curr_pos_local, parameters);

 	// Get the normalized switch distance.
-	float switch_distance_normalized = _npfg.switchDistance(FLT_MAX) * NORMALIZED_MAJOR_RADIUS / parameters.loiter_radius;
+	float switch_distance_normalized = _directional_guidance.switchDistance(FLT_MAX) * NORMALIZED_MAJOR_RADIUS /
+					   parameters.loiter_radius;

 	// Update segment if segment exit condition has been reached
 	switch (_current_segment) {
@@ -275,56 +277,60 @@ void FigureEight::updateSegment(const matrix::Vector2f &curr_pos_local, const Fi
 	}
 }

-void FigureEight::applyControl(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
-			       const FigureEightPatternParameters &parameters, float target_airspeed,
-			       const FigureEightPatternPoints &pattern_points)
+DirectionalGuidanceOutput FigureEight::applyControl(const matrix::Vector2f &curr_pos_local,
+		const matrix::Vector2f &ground_speed,
+		const FigureEightPatternParameters &parameters,
+		const FigureEightPatternPoints &pattern_points)
 {
 	Vector2f center_to_pos_local;
 	calculatePositionToCenterNormalizedRotated(center_to_pos_local, curr_pos_local, parameters);

 	switch (_current_segment) {
 	case FigureEightSegment::SEGMENT_CIRCLE_NORTH: {
-			applyCircle(NORTH_CIRCLE_IS_COUNTER_CLOCKWISE, pattern_points.normalized_north_circle_offset, curr_pos_local,
-				    ground_speed, parameters, target_airspeed);
+			return applyCircle(NORTH_CIRCLE_IS_COUNTER_CLOCKWISE, pattern_points.normalized_north_circle_offset, curr_pos_local,
+					   ground_speed, parameters);
 		}
 		break;

 	case FigureEightSegment::SEGMENT_NORTHEAST_SOUTHWEST: {
 			// Follow path from north-east to south-west
-			applyLine(pattern_points.normalized_north_exit_offset, pattern_points.normalized_south_entry_offset, curr_pos_local,
-				  ground_speed, parameters, target_airspeed);
+			return applyLine(pattern_points.normalized_north_exit_offset, pattern_points.normalized_south_entry_offset,
+					 curr_pos_local,
+					 ground_speed, parameters);
 		}
 		break;

 	case FigureEightSegment::SEGMENT_CIRCLE_SOUTH: {
-			applyCircle(SOUTH_CIRCLE_IS_COUNTER_CLOCKWISE, pattern_points.normalized_south_circle_offset, curr_pos_local,
-				    ground_speed, parameters, target_airspeed);
+			return applyCircle(SOUTH_CIRCLE_IS_COUNTER_CLOCKWISE, pattern_points.normalized_south_circle_offset, curr_pos_local,
+					   ground_speed, parameters);
 		}
 		break;

 	case FigureEightSegment::SEGMENT_SOUTHEAST_NORTHWEST: {
 			// follow path from south-east to north-west
-			applyLine(pattern_points.normalized_south_exit_offset, pattern_points.normalized_north_entry_offset, curr_pos_local,
-				  ground_speed, parameters, target_airspeed);
+			return applyLine(pattern_points.normalized_south_exit_offset, pattern_points.normalized_north_entry_offset,
+					 curr_pos_local,
+					 ground_speed, parameters);
 		}
 		break;

 	case FigureEightSegment::SEGMENT_POINT_SOUTHWEST: {
 			// Follow path from current position to south-west
-			applyLine(center_to_pos_local, pattern_points.normalized_south_entry_offset, curr_pos_local,
-				  ground_speed, parameters, target_airspeed);
+			return applyLine(center_to_pos_local, pattern_points.normalized_south_entry_offset, curr_pos_local,
+					 ground_speed, parameters);
 		}
 		break;

 	case FigureEightSegment::SEGMENT_POINT_NORTHWEST: {
 			// Follow path from current position to north-west
-			applyLine(center_to_pos_local, pattern_points.normalized_north_entry_offset, curr_pos_local,
-				  ground_speed, parameters, target_airspeed);
+			return applyLine(center_to_pos_local, pattern_points.normalized_north_entry_offset, curr_pos_local,
+					 ground_speed, parameters);
 		}
 		break;

 	case FigureEightSegment::SEGMENT_UNDEFINED:
 	default:
+		return DirectionalGuidanceOutput{};
 		break;
 	}
 }
@@ -356,9 +362,10 @@ float FigureEight::calculateRotationAngle(const FigureEightPatternParameters &pa
 	return yaw_rotation;
 }

-void FigureEight::applyCircle(bool loiter_direction_counter_clockwise, const matrix::Vector2f &normalized_circle_offset,
-			      const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
-			      const FigureEightPatternParameters &parameters, float target_airspeed)
+DirectionalGuidanceOutput FigureEight::applyCircle(bool loiter_direction_counter_clockwise,
+		const matrix::Vector2f &normalized_circle_offset,
+		const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
+		const FigureEightPatternParameters &parameters)
 {
 	const float loiter_direction_multiplier = loiter_direction_counter_clockwise ? -1.f : 1.f;

@@ -366,8 +373,6 @@ void FigureEight::applyCircle(bool loiter_direction_counter_clockwise, const mat
 	Vector2f circle_offset_rotated = Dcm2f(calculateRotationAngle(parameters)) * circle_offset;
 	Vector2f circle_center = parameters.center_pos_local + circle_offset_rotated;

-	_npfg.setAirspeedNom(target_airspeed * _eas2tas);
-	_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);

 	const Vector2f vector_center_to_vehicle = curr_pos_local - circle_center;
 	const float dist_to_center = vector_center_to_vehicle.norm();
@@ -392,16 +397,14 @@ void FigureEight::applyCircle(bool loiter_direction_counter_clockwise, const mat
 	float path_curvature = loiter_direction_multiplier / parameters.loiter_minor_radius;
 	_target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0));
 	_closest_point_on_path = unit_vec_center_to_closest_pt * parameters.loiter_minor_radius + circle_center;
-	_npfg.guideToPath(curr_pos_local, ground_speed, _wind_vel, unit_path_tangent,
-			  _closest_point_on_path, path_curvature);
+	return _directional_guidance.guideToPath(curr_pos_local, ground_speed, _wind_vel, unit_path_tangent,
+			_closest_point_on_path, path_curvature);

-	_roll_setpoint = _npfg.getRollSetpoint();
-	_indicated_airspeed_setpoint = _npfg.getAirspeedRef() / _eas2tas;
 }

-void FigureEight::applyLine(const matrix::Vector2f &normalized_line_start_offset,
-			    const matrix::Vector2f &normalized_line_end_offset, const matrix::Vector2f &curr_pos_local,
-			    const matrix::Vector2f &ground_speed, const FigureEightPatternParameters &parameters, float target_airspeed)
+DirectionalGuidanceOutput FigureEight::applyLine(const matrix::Vector2f &normalized_line_start_offset,
+		const matrix::Vector2f &normalized_line_end_offset, const matrix::Vector2f &curr_pos_local,
+		const matrix::Vector2f &ground_speed, const FigureEightPatternParameters &parameters)
 {
 	const Dcm2f rotation_matrix(calculateRotationAngle(parameters));

@@ -414,15 +417,12 @@ void FigureEight::applyLine(const matrix::Vector2f &normalized_line_start_offset
 	const Vector2f end_offset_rotated = rotation_matrix * end_offset;
 	const Vector2f line_segment_end_position = parameters.center_pos_local + end_offset_rotated;

-	_npfg.setAirspeedNom(target_airspeed * _eas2tas);
-	_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
 	const Vector2f path_tangent = line_segment_end_position - line_segment_start_position;
 	const Vector2f unit_path_tangent = path_tangent.normalized();
 	_target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0));
 	const Vector2f vector_A_to_vehicle = curr_pos_local - line_segment_start_position;
 	_closest_point_on_path = line_segment_start_position + vector_A_to_vehicle.dot(unit_path_tangent) * unit_path_tangent;
-	_npfg.guideToPath(curr_pos_local, ground_speed, _wind_vel, path_tangent.normalized(), line_segment_start_position,
-			  0.0f);
-	_roll_setpoint = _npfg.getRollSetpoint();
-	_indicated_airspeed_setpoint = _npfg.getAirspeedRef() / _eas2tas;
+	return _directional_guidance.guideToPath(curr_pos_local, ground_speed, _wind_vel, path_tangent.normalized(),
+			line_segment_start_position,
+			0.0f);
 }
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *   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
@@ -45,7 +45,7 @@
 #include <px4_platform_common/param.h>
 #include <lib/matrix/matrix/math.hpp>

-#include "lib/npfg/npfg.hpp"
+#include "lib/npfg/DirectionalGuidance.hpp"

 class FigureEight : public ModuleParams
 {
@@ -87,9 +87,8 @@ public:
 	 *
 	 * @param[in] npfg is the reference to the parent npfg object.
 	 * @param[in] wind_vel is the reference to the parent wind velocity [m/s].
-	 * @param[in] eas2tas is the reference to the parent indicated airspeed to true airspeed conversion.
 	 */
-	FigureEight(NPFG &npfg, matrix::Vector2f &wind_vel, float &eas2tas);
+	FigureEight(DirectionalGuidance &directional_guidance, matrix::Vector2f &wind_vel);

 	/**
 	 * @brief reset the figure eight pattern.
@@ -100,27 +99,14 @@ public:
 	void resetPattern();

 	/**
-	 * @brief Update roll and airspeed setpoint.
+	 * @brief Update roll setpoint
 	 *
 	 * @param[in] curr_pos_local is the current local position of the vehicle in [m].
 	 * @param[in] ground_speed is the current ground speed of the vehicle in [m/s].
 	 * @param[in] parameters is the parameter set defining the figure eight shape.
-	 * @param[in] target_airspeed is the current targeted indicated airspeed [m/s].
 	 */
-	void updateSetpoint(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
-			    const FigureEightPatternParameters &parameters, float target_airspeed);
-	/**
-	 * @brief Get the roll setpoint
-	 *
-	 * @return the roll setpoint in [rad].
-	 */
-	float getRollSetpoint() const {return _roll_setpoint;};
-	/**
-	 * @brief Get the indicated airspeed setpoint
-	 *
-	 * @return the indicated airspeed setpoint in [m/s].
-	 */
-	float getAirspeedSetpoint() const {return _indicated_airspeed_setpoint;};
+	DirectionalGuidanceOutput updateSetpoint(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
+			const FigureEightPatternParameters &parameters);
 	/**
 	 * @brief Get the target bearing of current point on figure of eight
 	 *
@@ -134,7 +120,6 @@ public:
 	 */
 	matrix::Vector2f getClosestPoint() const {return _closest_point_on_path;};

-
 private:
 	/**
 	 * @brief
@@ -172,12 +157,11 @@ private:
 	 * @param[in] curr_pos_local is the current local position of the vehicle in [m].
 	 * @param[in] ground_speed is the current ground speed of the vehicle in [m/s].
 	 * @param[in] parameters is the parameter set defining the figure eight shape.
-	 * @param[in] target_airspeed is the current targeted indicated airspeed [m/s].
 	 * @param[in] pattern_points are the relevant points defining the figure eight pattern.
 	 */
-	void applyControl(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
-			  const FigureEightPatternParameters &parameters, float target_airspeed,
-			  const FigureEightPatternPoints &pattern_points);
+	DirectionalGuidanceOutput applyControl(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
+					       const FigureEightPatternParameters &parameters,
+					       const FigureEightPatternPoints &pattern_points);
 	/**
 	 * @brief Update active segment.
 	 *
@@ -212,11 +196,11 @@ private:
 	 * @param[in] curr_pos_local is the current local position of the vehicle in [m].
 	 * @param[in] ground_speed is the current ground speed of the vehicle in [m/s].
 	 * @param[in] parameters is the parameter set defining the figure eight shape.
-	 * @param[in] target_airspeed is the current targeted indicated airspeed [m/s].
 	 */
-	void applyCircle(bool loiter_direction_counter_clockwise, const matrix::Vector2f &normalized_circle_offset,
-			 const matrix::Vector2f &curr_pos_local,
-			 const matrix::Vector2f &ground_speed, const FigureEightPatternParameters &parameters, float target_airspeed);
+	DirectionalGuidanceOutput applyCircle(bool loiter_direction_counter_clockwise,
+					      const matrix::Vector2f &normalized_circle_offset,
+					      const matrix::Vector2f &curr_pos_local,
+					      const matrix::Vector2f &ground_speed, const FigureEightPatternParameters &parameters);
 	/**
 	 * @brief Apply path lateral control
 	 *
@@ -225,18 +209,18 @@ private:
 	 * @param[in] curr_pos_local is the current local position of the vehicle in [m].
 	 * @param[in] ground_speed is the current ground speed of the vehicle in [m/s].
 	 * @param[in] parameters is the parameter set defining the figure eight shape.
-	 * @param[in] target_airspeed is the current targeted indicated airspeed [m/s].
 	 */
-	void applyLine(const matrix::Vector2f &normalized_line_start_offset, const matrix::Vector2f &normalized_line_end_offset,
-		       const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
-		       const FigureEightPatternParameters &parameters, float target_airspeed);
+	DirectionalGuidanceOutput applyLine(const matrix::Vector2f &normalized_line_start_offset,
+					    const matrix::Vector2f &normalized_line_end_offset,
+					    const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
+					    const FigureEightPatternParameters &parameters);

 private:
 	/**
 	 * @brief npfg lateral control object.
 	 *
 	 */
-	NPFG &_npfg;
+	DirectionalGuidance &_directional_guidance;

 	/**
 	 * @brief Wind velocity in [m/s].
@@ -244,24 +228,9 @@ private:
 	 */
 	const matrix::Vector2f &_wind_vel;
 	/**
-	 * @brief Conversion factor from indicated to true airspeed.
-	 *
-	 */
-	const float &_eas2tas;
-	/**
-	 * @brief Roll setpoint in [rad].
-	 *
-	 */
-	float _roll_setpoint;
-	/**
-	 * @brief Indicated airspeed setpoint in [m/s].
-	 *
-	 */
-	float _indicated_airspeed_setpoint;
-	/**
-	 * @brief active figure eight position setpoint.
-	 *
-	 */
+	* @brief active figure eight position setpoint.
+	*
+	*/
 	FigureEightPatternParameters _active_parameters;

 	/**
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013-2023 PX4 Development Team. All rights reserved.
+ *   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
@@ -31,20 +31,6 @@
 *
 ****************************************************************************/

-/**
- * Path navigation roll slew rate limit.
- *
- * Maximum change in roll angle setpoint per second.
- * Applied in all Auto modes, plus manual Position & Altitude modes.
- *
- * @unit deg/s
- * @min 0
- * @decimal 0
- * @increment 1
- * @group FW Path Control
- */
-PARAM_DEFINE_FLOAT(FW_PN_R_SLEW_MAX, 90.0f);
-
 /**
 * NPFG period
 *
@@ -93,48 +79,6 @@ PARAM_DEFINE_INT32(NPFG_LB_PERIOD, 1);
 */
 PARAM_DEFINE_INT32(NPFG_UB_PERIOD, 1);

-/**
- * Enable track keeping excess wind handling logic.
- *
- * @boolean
- * @group FW NPFG Control
- */
-PARAM_DEFINE_INT32(NPFG_TRACK_KEEP, 1);
-
-/**
- * Enable minimum forward ground speed maintaining excess wind handling logic
- *
- * @boolean
- * @group FW NPFG Control
- */
-PARAM_DEFINE_INT32(NPFG_EN_MIN_GSP, 1);
-
-/**
- * Enable wind excess regulation.
- *
- * Disabling this parameter further disables all other airspeed incrementation options.
- *
- * @boolean
- * @group FW NPFG Control
- */
-PARAM_DEFINE_INT32(NPFG_WIND_REG, 1);
-
-/**
- * Maximum, minimum forward ground speed for track keeping in excess wind
- *
- * The maximum value of the minimum forward ground speed that may be commanded
- * by the track keeping excess wind handling logic. Commanded in full at the normalized
- * track error fraction of the track error boundary and reduced to zero on track.
- *
- * @unit m/s
- * @min 0.0
- * @max 10.0
- * @decimal 1
- * @increment 0.5
- * @group FW NPFG Control
- */
-PARAM_DEFINE_FLOAT(NPFG_GSP_MAX_TK, 5.0f);
-
 /**
 * Roll time constant
 *
@@ -178,20 +122,6 @@ PARAM_DEFINE_FLOAT(NPFG_SW_DST_MLT, 0.32f);
 */
 PARAM_DEFINE_FLOAT(NPFG_PERIOD_SF, 1.5f);

-
-/**
- * Throttle max slew rate
- *
- * Maximum slew rate for the commanded throttle
- *
- * @min 0.0
- * @max 1.0
- * @decimal 2
- * @increment 0.01
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_THR_SLEW_MAX, 0.0f);
-
 /**
 * Minimum pitch angle setpoint
 *
@@ -423,157 +353,6 @@ PARAM_DEFINE_FLOAT(FW_LND_AIRSPD, -1.f);
 */
 PARAM_DEFINE_FLOAT(FW_LND_THRTC_SC, 1.0f);

-/**
- * Low-height threshold for tighter altitude tracking
- *
- * Height above ground threshold below which tighter altitude
- * tracking gets enabled (see FW_LND_THRTC_SC). Below this height, TECS smoothly
- * (1 sec / sec) transitions the altitude tracking time constant from FW_T_ALT_TC
- * to FW_LND_THRTC_SC*FW_T_ALT_TC.
- *
- * -1 to disable.
- *
- * @unit m
- * @min -1
- * @decimal 0
- * @increment 1
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_T_THR_LOW_HGT, -1.f);
-
-/*
- * TECS parameters
- *
- */
-
-/**
- * Maximum descent rate
- *
- * @unit m/s
- * @min 1.0
- * @max 15.0
- * @decimal 1
- * @increment 0.5
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
-
-/**
- * Throttle damping factor
- *
- * This is the damping gain for the throttle demand loop.
- *
- * @min 0.0
- * @max 1.0
- * @decimal 3
- * @increment 0.01
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_T_THR_DAMPING, 0.05f);
-
-/**
- * Integrator gain throttle
- *
- * Increase it to trim out speed and height offsets faster,
- * with the downside of possible overshoots and oscillations.
- *
- * @min 0.0
- * @max 1.0
- * @decimal 3
- * @increment 0.005
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_T_THR_INTEG, 0.02f);
-
-/**
- * Integrator gain pitch
- *
- * Increase it to trim out speed and height offsets faster,
- * with the downside of possible overshoots and oscillations.
- *
- * @min 0.0
- * @max 2.0
- * @decimal 2
- * @increment 0.05
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_T_I_GAIN_PIT, 0.1f);
-
-/**
- * Maximum vertical acceleration
- *
- * This is the maximum vertical acceleration
- * either up or down that the controller will use to correct speed
- * or height errors.
- *
- * @unit m/s^2
- * @min 1.0
- * @max 10.0
- * @decimal 1
- * @increment 0.5
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
-
-/**
- * Airspeed measurement standard deviation
- *
- * For the airspeed filter in TECS.
- *
- * @unit m/s
- * @min 0.01
- * @max 10.0
- * @decimal 2
- * @increment 0.1
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_T_SPD_STD, 0.07f);
-
-/**
- * Airspeed rate measurement standard deviation
- *
- * For the airspeed filter in TECS.
- *
- * @unit m/s^2
- * @min 0.01
- * @max 10.0
- * @decimal 2
- * @increment 0.1
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_T_SPD_DEV_STD, 0.2f);
-
-/**
- * Process noise standard deviation for the airspeed rate
- *
- * This is defining the noise in the airspeed rate for the constant airspeed rate model
- * of the TECS airspeed filter.
- *
- * @unit m/s^2
- * @min 0.01
- * @max 10.0
- * @decimal 2
- * @increment 0.1
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_T_SPD_PRC_STD, 0.2f);
-
-
-/**
- * Roll -> Throttle feedforward
- *
- * Is used to compensate for the additional drag created by turning.
- * Increase this gain if the aircraft initially loses energy in turns
- * and reduce if the aircraft initially gains energy in turns.
- *
- * @min 0.0
- * @max 20.0
- * @decimal 1
- * @increment 0.5
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 15.0f);
-
 /**
 * Speed <--> Altitude weight
 *
@@ -590,75 +369,6 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 15.0f);
 */
 PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);

-/**
- * Pitch damping gain
- *
- * @min 0.0
- * @max 2.0
- * @decimal 2
- * @increment 0.1
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.1f);
-
-/**
- * Altitude error time constant.
- *
- * @min 2.0
- * @decimal 2
- * @increment 0.5
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_T_ALT_TC, 5.0f);
-
-/**
- * Fast descend: minimum altitude error
- *
- * Minimum altitude error needed to descend with max airspeed and minimal throttle.
- * A negative value disables fast descend.
- *
- * @min -1.0
- * @decimal 0
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_T_F_ALT_ERR, -1.0f);
-
-/**
- * Height rate feed forward
- *
- * @min 0.0
- * @max 1.0
- * @decimal 2
- * @increment 0.05
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.3f);
-
-/**
- * True airspeed error time constant.
- *
- * @min 2.0
- * @decimal 2
- * @increment 0.5
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_T_TAS_TC, 5.0f);
-
-/**
- * Minimum groundspeed
- *
- * The controller will increase the commanded airspeed to maintain
- * this minimum groundspeed to the next waypoint.
- *
- * @unit m/s
- * @min 0.0
- * @max 40
- * @decimal 1
- * @increment 0.5
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_GND_SPD_MIN, 5.0f);
-
 /**
 * Custom stick configuration
 *
@@ -672,31 +382,6 @@ PARAM_DEFINE_FLOAT(FW_GND_SPD_MIN, 5.0f);
 */
 PARAM_DEFINE_INT32(FW_POS_STK_CONF, 2);

-/**
- * Specific total energy rate first order filter time constant.
- *
- * This filter is applied to the specific total energy rate used for throttle damping.
- *
- * @min 0.0
- * @max 2
- * @decimal 2
- * @increment 0.01
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_T_STE_R_TC, 0.4f);
-
-/**
- * Specific total energy balance rate feedforward gain.
- *
- *
- * @min 0.5
- * @max 3
- * @decimal 2
- * @increment 0.01
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_T_SEB_R_FF, 1.0f);
-
 /**
 * Default target climbrate.
 *
@@ -886,20 +571,6 @@ PARAM_DEFINE_INT32(FW_LND_NUDGE, 2);
 */
 PARAM_DEFINE_INT32(FW_LND_ABORT, 3);

-/**
- * Wind-based airspeed scaling factor
- *
- * Multiplying this factor with the current absolute wind estimate gives the airspeed offset
- * added to the minimum airspeed setpoint limit. This helps to make the
- * system more robust against disturbances (turbulence) in high wind.
- *
- * @min 0
- * @decimal 2
- * @increment 0.01
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_WIND_ARSP_SC, 0.f);
-
 /**
 * Fixed-wing launch detection
 *
@@ -94,7 +94,6 @@ void LoggedTopics::add_default_topics()
 	add_topic("mission_result");
 	add_topic("navigator_mission_item");
 	add_topic("navigator_status");
-	add_topic("npfg_status", 100);
 	add_topic("offboard_control_mode", 100);
 	add_topic("onboard_computer_status", 10);
 	add_topic("parameter_update");
@@ -149,6 +148,12 @@ void LoggedTopics::add_default_topics()
 	add_topic("vehicle_status");
 	add_optional_topic("vtol_vehicle_status", 200);
 	add_topic("wind", 1000);
+	add_topic("fixed_wing_lateral_setpoint");
+	add_topic("fixed_wing_longitudinal_setpoint");
+	add_topic("longitudinal_control_configuration");
+	add_topic("lateral_control_configuration");
+	add_optional_topic("fixed_wing_lateral_guidance_status", 100);
+	add_optional_topic("fixed_wing_lateral_status", 100);

 	// multi topics
 	add_optional_topic_multi("actuator_outputs", 100, 3);
@@ -445,10 +445,6 @@ menu "Zenoh publishers/subscribers"
 		bool "normalized_unsigned_setpoint"
 		default n

-	config ZENOH_PUBSUB_NPFG_STATUS
-		bool "npfg_status"
-		default n
-
 	config ZENOH_PUBSUB_OBSTACLE_DISTANCE
 		bool "obstacle_distance"
 		default n