feat(fw_performance_model): scale vehicle weight by remaining fuel

msg/TecsStatus.msg

@@ -29,3 +29,5 @@ float32 throttle_trim			# estimated throttle value [0,1] required to fly level a
 
 float32 underspeed_ratio		# 0: no underspeed, 1: maximal underspeed. Controller takes measures to avoid stall proportional to ratio if >0.
 float32 fast_descend_ratio 		#  value indicating if fast descend mode is enabled with ramp up and ramp down [0-1]
+
+float32 weight_ratio			# Ratio of estimated vehicle weight to WEIGHT_BASE, accounting for burned fuel if fuel-based weight compensation is enabled [-]

src/lib/fw_performance_model/CMakeLists.txt

@@ -34,4 +34,7 @@
 px4_add_library(performance_model
         PerformanceModel.cpp
 )
+target_link_libraries(performance_model PRIVATE atmosphere geo)
 set_property(GLOBAL APPEND PROPERTY PX4_MODULE_CONFIG_FILES ${CMAKE_CURRENT_SOURCE_DIR}/performance_model_params.yaml)
+
+px4_add_functional_gtest(SRC PerformanceModelTest.cpp LINKLIBS performance_model)

src/lib/fw_performance_model/PerformanceModel.cpp

@@ -58,12 +58,42 @@ PerformanceModel::PerformanceModel(): ModuleParams(nullptr)
 {
 	updateParams();
 }
+void PerformanceModel::setFuelFractionRemaining(float fuel_fraction_remaining)
+{
+	if (PX4_ISFINITE(fuel_fraction_remaining)) {
+		_fuel_fraction_remaining = math::constrain(fuel_fraction_remaining, 0.f, 1.f);
+	}
+}
+
+float PerformanceModel::getFuelFractionRemaining(const fuel_tank_status_s &fuel_tank_status)
+{
+	float fraction = NAN;
+
+	if (PX4_ISFINITE(fuel_tank_status.remaining_fuel) && fuel_tank_status.maximum_fuel_capacity > FLT_EPSILON) {
+		fraction = fuel_tank_status.remaining_fuel / fuel_tank_status.maximum_fuel_capacity;
+
+	} else if (fuel_tank_status.percent_remaining <= 100) {
+		fraction = fuel_tank_status.percent_remaining * 0.01f;
+
+	} else if (PX4_ISFINITE(fuel_tank_status.consumed_fuel) && fuel_tank_status.maximum_fuel_capacity > FLT_EPSILON) {
+		fraction = 1.f - fuel_tank_status.consumed_fuel / fuel_tank_status.maximum_fuel_capacity;
+	}
+
+	return PX4_ISFINITE(fraction) ? math::constrain(fraction, 0.f, 1.f) : NAN;
+}
+
 float PerformanceModel::getWeightRatio() const
 {
 	float weight_factor = 1.0f;
 
 	if (_param_weight_base.get() > FLT_EPSILON && _param_weight_gross.get() > FLT_EPSILON) {
-		weight_factor = math::constrain(_param_weight_gross.get() / _param_weight_base.get(), kMinWeightRatio,
+		float weight = _param_weight_gross.get();
+
+		if (_param_weight_fuel.get() > FLT_EPSILON && PX4_ISFINITE(_fuel_fraction_remaining)) {
+			weight -= (1.f - _fuel_fraction_remaining) * _param_weight_fuel.get();
+		}
+
+		weight_factor = math::constrain(weight / _param_weight_base.get(), kMinWeightRatio,
 						kMaxWeightRatio);
 	}
 

src/lib/fw_performance_model/PerformanceModel.hpp

@@ -37,7 +37,9 @@
  * Performance model.
  */
 
+#include <math.h>
 #include <px4_platform_common/module_params.h>
+#include <uORB/topics/fuel_tank_status.h>
 
 #ifndef PX4_SRC_MODULES_FW_POS_CONTROL_PERFORMANCEMODEL_H_
 #define PX4_SRC_MODULES_FW_POS_CONTROL_PERFORMANCEMODEL_H_
@@ -70,6 +72,23 @@ class PerformanceModel : public ModuleParams
 	 */
 	float getWeightRatio() const;
 
+	/**
+	 * Set the fraction of fuel remaining, used to reduce the vehicle weight by the burned fuel mass
+	 * if fuel-based weight compensation is enabled (WEIGHT_FUEL > 0).
+	 * Non-finite input is ignored and the last known fuel fraction is kept.
+	 * @param fuel_fraction_remaining fraction of fuel remaining in range [0,1]
+	 */
+	void setFuelFractionRemaining(float fuel_fraction_remaining);
+
+	/**
+	 * Extract the fraction of fuel remaining from a fuel tank status message, preferring the
+	 * measured remaining fuel over the remaining percentage over the consumed fuel.
+	 * The consumed fuel fallback assumes the tank was full at boot.
+	 * @param fuel_tank_status fuel tank status message
+	 * @return fraction of fuel remaining in range [0,1], NAN if not available
+	 */
+	static float getFuelFractionRemaining(const fuel_tank_status_s &fuel_tank_status);
+
 	/**
 	 * Get the trim throttle for the current airspeed setpoint as well as air density and weight.
 	 * @param throttle_min minimum throttle in range [0,1]
@@ -131,6 +150,7 @@ class PerformanceModel : public ModuleParams
 		(ParamFloat<px4::params::FW_T_SINK_MIN>) _param_fw_t_sink_min,
 		(ParamFloat<px4::params::WEIGHT_BASE>) _param_weight_base,
 		(ParamFloat<px4::params::WEIGHT_GROSS>) _param_weight_gross,
+		(ParamFloat<px4::params::WEIGHT_FUEL>) _param_weight_fuel,
 		(ParamFloat<px4::params::FW_SERVICE_CEIL>) _param_service_ceiling,
 		(ParamFloat<px4::params::FW_THR_TRIM>) _param_fw_thr_trim,
 		(ParamFloat<px4::params::FW_THR_MAX>) _param_fw_thr_max,
@@ -150,6 +170,8 @@ class PerformanceModel : public ModuleParams
 
 
 	static float sanitiseAirDensity(float air_density);
+
+	float _fuel_fraction_remaining{NAN}; ///< fraction of fuel remaining in range [0,1], NAN if unknown
 };
 
 #endif //PX4_SRC_MODULES_FW_POS_CONTROL_PERFORMANCEMODEL_H_

src/lib/fw_performance_model/PerformanceModelTest.cpp

@@ -0,0 +1,181 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2026 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 <gtest/gtest.h>
+#include "PerformanceModel.hpp"
+
+// to run: make tests TESTFILTER=PerformanceModel
+
+class PerformanceModelTest : public ::testing::Test
+{
+public:
+	void SetUp() override
+	{
+		param_control_autosave(false);
+		param_reset_all();
+	}
+
+	void setWeightParams(float base, float gross, float fuel)
+	{
+		param_set(param_handle(px4::params::WEIGHT_BASE), &base);
+		param_set(param_handle(px4::params::WEIGHT_GROSS), &gross);
+		param_set(param_handle(px4::params::WEIGHT_FUEL), &fuel);
+		_model.updateParameters();
+	}
+
+	PerformanceModel _model;
+};
+
+TEST_F(PerformanceModelTest, weightRatioDefaultsToOne)
+{
+	// GIVEN: default parameters (weight compensation disabled)
+	// THEN: the weight ratio is 1
+	EXPECT_FLOAT_EQ(_model.getWeightRatio(), 1.0f);
+}
+
+TEST_F(PerformanceModelTest, weightRatioWithoutFuelCompensation)
+{
+	// GIVEN: base and gross weight set, fuel compensation disabled
+	setWeightParams(10.0f, 12.0f, -1.0f);
+
+	// THEN: the weight ratio is gross/base, independent of any fuel fraction
+	EXPECT_FLOAT_EQ(_model.getWeightRatio(), 1.2f);
+
+	_model.setFuelFractionRemaining(0.5f);
+	EXPECT_FLOAT_EQ(_model.getWeightRatio(), 1.2f);
+}
+
+TEST_F(PerformanceModelTest, weightRatioScalesWithFuelFraction)
+{
+	// GIVEN: base and gross weight set, 4kg full fuel load
+	setWeightParams(10.0f, 12.0f, 4.0f);
+
+	// THEN: with unknown fuel fraction the full-tank gross weight is assumed
+	EXPECT_FLOAT_EQ(_model.getWeightRatio(), 1.2f);
+
+	// AND: the weight ratio scales linearly with the burned fuel mass
+	_model.setFuelFractionRemaining(1.0f);
+	EXPECT_FLOAT_EQ(_model.getWeightRatio(), 1.2f);
+
+	_model.setFuelFractionRemaining(0.5f);
+	EXPECT_FLOAT_EQ(_model.getWeightRatio(), 1.0f);
+
+	_model.setFuelFractionRemaining(0.0f);
+	EXPECT_FLOAT_EQ(_model.getWeightRatio(), 0.8f);
+}
+
+TEST_F(PerformanceModelTest, fuelFractionKeptOnNonFiniteInput)
+{
+	// GIVEN: base and gross weight set, 4kg full fuel load, and a known fuel fraction
+	setWeightParams(10.0f, 12.0f, 4.0f);
+	_model.setFuelFractionRemaining(0.5f);
+	EXPECT_FLOAT_EQ(_model.getWeightRatio(), 1.0f);
+
+	// WHEN: the fuel fraction input becomes non-finite (e.g. sensor failure)
+	_model.setFuelFractionRemaining(NAN);
+
+	// THEN: the last known fuel fraction is kept
+	EXPECT_FLOAT_EQ(_model.getWeightRatio(), 1.0f);
+}
+
+TEST_F(PerformanceModelTest, fuelFractionIsConstrained)
+{
+	// GIVEN: base and gross weight set, 4kg full fuel load
+	setWeightParams(10.0f, 12.0f, 4.0f);
+
+	// THEN: out-of-range fuel fractions are constrained to [0, 1]
+	_model.setFuelFractionRemaining(-0.5f);
+	EXPECT_FLOAT_EQ(_model.getWeightRatio(), 0.8f);
+
+	_model.setFuelFractionRemaining(1.5f);
+	EXPECT_FLOAT_EQ(_model.getWeightRatio(), 1.2f);
+}
+
+TEST_F(PerformanceModelTest, weightRatioIsConstrained)
+{
+	// GIVEN: a fuel weight larger than the gross weight (misconfiguration)
+	setWeightParams(10.0f, 12.0f, 12.0f);
+
+	// THEN: the weight ratio is constrained to its lower bound
+	_model.setFuelFractionRemaining(0.0f);
+	EXPECT_FLOAT_EQ(_model.getWeightRatio(), 0.5f);
+}
+
+TEST_F(PerformanceModelTest, fuelFractionFromTankStatusFallbackChain)
+{
+	// GIVEN: a fuel tank status with no measurements
+	fuel_tank_status_s status{};
+	status.maximum_fuel_capacity = NAN;
+	status.remaining_fuel = NAN;
+	status.consumed_fuel = NAN;
+	status.percent_remaining = UINT8_MAX;
+
+	// THEN: no fuel fraction is available
+	EXPECT_TRUE(std::isnan(PerformanceModel::getFuelFractionRemaining(status)));
+
+	// AND: the consumed fuel is the last fallback, assuming a full tank at boot
+	status.maximum_fuel_capacity = 2000.f;
+	status.consumed_fuel = 500.f;
+	EXPECT_FLOAT_EQ(PerformanceModel::getFuelFractionRemaining(status), 0.75f);
+
+	// AND: a valid remaining percentage takes precedence over the consumed fuel
+	status.percent_remaining = 40;
+	EXPECT_FLOAT_EQ(PerformanceModel::getFuelFractionRemaining(status), 0.4f);
+
+	// AND: the measured remaining fuel takes precedence over everything else
+	status.remaining_fuel = 1000.f;
+	EXPECT_FLOAT_EQ(PerformanceModel::getFuelFractionRemaining(status), 0.5f);
+}
+
+TEST_F(PerformanceModelTest, fuelFractionFromTankStatusIsConstrained)
+{
+	// GIVEN: a remaining fuel measurement exceeding the maximum capacity
+	fuel_tank_status_s status{};
+	status.maximum_fuel_capacity = 2000.f;
+	status.remaining_fuel = 3000.f;
+	status.consumed_fuel = NAN;
+	status.percent_remaining = UINT8_MAX;
+
+	// THEN: the fuel fraction is constrained to 1
+	EXPECT_FLOAT_EQ(PerformanceModel::getFuelFractionRemaining(status), 1.0f);
+
+	// AND: more consumed fuel than the maximum capacity results in a fraction of 0
+	status.remaining_fuel = NAN;
+	status.consumed_fuel = 3000.f;
+	EXPECT_FLOAT_EQ(PerformanceModel::getFuelFractionRemaining(status), 0.0f);
+
+	// AND: a remaining fuel measurement without a valid maximum capacity is unusable
+	status.maximum_fuel_capacity = 0.f;
+	status.remaining_fuel = 1000.f;
+	EXPECT_TRUE(std::isnan(PerformanceModel::getFuelFractionRemaining(status)));
+}

src/lib/fw_performance_model/performance_model_params.yaml

@@ -71,6 +71,19 @@ parameters:
           This is the actual weight of the vehicle at any time. This value will differ from WEIGHT_BASE in case weight was added
           or removed from the base weight. Examples are the addition of payloads or larger batteries. A zero or negative value
           disables trim throttle and minimum airspeed compensation based on weight.
+          If fuel-based weight compensation is enabled (WEIGHT_FUEL), set this to the takeoff weight with a full fuel load.
+      type: float
+      default: -1.0
+      unit: kg
+      decimal: 1
+      increment: 0.1
+    WEIGHT_FUEL:
+      description:
+        short: Weight of a full fuel load
+        long: |-
+          Mass of a full fuel load. If set together with WEIGHT_BASE and WEIGHT_GROSS, the weight used for
+          performance scaling is reduced by the burned fuel reported in the fuel tank status.
+          Only a single fuel tank is supported. A zero or negative value disables fuel-based weight compensation.
       type: float
       default: -1.0
       unit: kg

src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp

@@ -66,6 +66,12 @@ 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;
 
+// [s] time constant of the fuel fraction filter, needs to be slow enough to reject fuel sloshing
+static constexpr float FUEL_FRACTION_FILTER_TIME_CONST = 30.f;
+
+// [s] maximum time step used to advance the fuel fraction filter on a new sample
+static constexpr float FUEL_FRACTION_FILTER_MAX_DT = 10.f;
+
 FwLateralLongitudinalControl::FwLateralLongitudinalControl(bool is_vtol) :
 	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
@@ -76,6 +82,9 @@ FwLateralLongitudinalControl::FwLateralLongitudinalControl(bool is_vtol) :
 	_tecs_status_pub.advertise();
 	_flight_phase_estimation_pub.advertise();
 	_fixed_wing_lateral_status_pub.advertise();
+
+	_fuel_fraction_filter.setParameters(0.f, FUEL_FRACTION_FILTER_TIME_CONST);
+
 	parameters_update();
 	_airspeed_slew_rate_controller.setSlewRate(ASPD_SP_SLEW_RATE);
 
@@ -160,6 +169,8 @@ void FwLateralLongitudinalControl::Run()
 			_tecs.set_min_sink_rate(_performance_model.getMinimumSinkRate(_air_density));
 		}
 
+		updateFuelState();
+
 		if (_vehicle_landed_sub.updated()) {
 			vehicle_land_detected_s landed{};
 			_vehicle_landed_sub.copy(&landed);
@@ -443,6 +454,45 @@ FwLateralLongitudinalControl::tecs_update_pitch_throttle(const float control_int
 	return flight_phase_estimation_s::FLIGHT_PHASE_UNKNOWN;
 }
 
+void FwLateralLongitudinalControl::updateFuelState()
+{
+	fuel_tank_status_s fuel_tank_status;
+
+	if (_fuel_tank_status_sub.update(&fuel_tank_status)) {
+		// only a single fuel tank is supported: use the first reported tank id and ignore all others
+		if (_fuel_tank_id < 0) {
+			_fuel_tank_id = fuel_tank_status.fuel_tank_id;
+		}
+
+		if (fuel_tank_status.fuel_tank_id != _fuel_tank_id) {
+			return;
+		}
+
+		const float fuel_fraction_remaining = PerformanceModel::getFuelFractionRemaining(fuel_tank_status);
+
+		if (PX4_ISFINITE(fuel_fraction_remaining)) {
+			if (_time_last_fuel_fraction_update == 0) {
+				_fuel_fraction_filter.reset(fuel_fraction_remaining);
+
+			} else if (fuel_tank_status.timestamp > _time_last_fuel_fraction_update) {
+				const float dt = math::min((fuel_tank_status.timestamp - _time_last_fuel_fraction_update) * 1e-6f,
+							   FUEL_FRACTION_FILTER_MAX_DT);
+				_fuel_fraction_filter.update(fuel_fraction_remaining, dt);
+
+			} else {
+				// out-of-order sample
+				return;
+			}
+
+			_time_last_fuel_fraction_update = fuel_tank_status.timestamp;
+
+			_performance_model.setFuelFractionRemaining(_fuel_fraction_filter.getState());
+
+			_tecs.set_equivalent_airspeed_trim(_performance_model.getCalibratedTrimAirspeed());
+		}
+	}
+}
+
 void
 FwLateralLongitudinalControl::tecs_status_publish(float alt_sp, float equivalent_airspeed_sp,
 		float true_airspeed_derivative_raw, float throttle_trim, hrt_abstime timestamp)
@@ -475,6 +525,7 @@ FwLateralLongitudinalControl::tecs_status_publish(float alt_sp, float equivalent
 	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.weight_ratio = _performance_model.getWeightRatio();
 
 	tecs_status.timestamp = timestamp;