Added PerformanceModel for fixed wing (#22091)

* created a Performance Model for fixed wing vehicle
- added compensation for maximum climbrate, minimum sinkrate, minimum airspeed and trim airspeed based on weight ratio and air density
- added atmosphere lib to standard atmosphere calculations


---------

Signed-off-by: RomanBapst <bapstroman@gmail.com>
Co-authored-by: Thomas Stastny <thomas.stastny@auterion.com>

msg/VehicleAirData.msg

@@ -10,5 +10,6 @@ float32 baro_temp_celcius		# Temperature in degrees Celsius
 float32 baro_pressure_pa		# Absolute pressure in Pascals
 
 float32 rho				# air density
+float32 eas2tas         # equivalent airspeed to true airspeed conversion factor
 
 uint8 calibration_count     # Calibration changed counter. Monotonically increases whenever calibration changes.

src/drivers/batt_smbus/batt_smbus.cpp

@@ -44,6 +44,7 @@
  */
 
 #include "batt_smbus.h"
+#include <lib/atmosphere/atmosphere.h>
 
 extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]);
 
@@ -160,7 +161,7 @@ void BATT_SMBUS::RunImpl()
 
 	// Read battery temperature and covert to Celsius.
 	ret |= _interface->read_word(BATT_SMBUS_TEMP, result);
-	new_report.temperature = ((float)result / 10.0f) + CONSTANTS_ABSOLUTE_NULL_CELSIUS;
+	new_report.temperature = ((float)result / 10.0f) + atmosphere::kAbsoluteNullCelsius;
 
 	// Only publish if no errors.
 	if (ret == PX4_OK) {

src/drivers/smart_battery/batmon/batmon.cpp

@@ -43,6 +43,7 @@
 
 #include "batmon.h"
 #include <mathlib/mathlib.h>
+#include <lib/atmosphere/atmosphere.h>
 
 extern "C" __EXPORT int batmon_main(int argc, char *argv[]);
 
@@ -186,7 +187,7 @@ void Batmon::RunImpl()
 
 	// Read battery temperature and covert to Celsius.
 	ret |= _interface->read_word(BATT_SMBUS_TEMP, result);
-	new_report.temperature = ((float)result / 10.0f) + CONSTANTS_ABSOLUTE_NULL_CELSIUS;
+	new_report.temperature = ((float)result / 10.0f) + atmosphere::kAbsoluteNullCelsius;
 
 	// Only publish if no errors.
 	if (ret == PX4_OK) {

src/drivers/uavcan/sensors/airspeed.cpp

@@ -38,7 +38,7 @@
 #include <drivers/drv_hrt.h>
 #include "airspeed.hpp"
 #include <math.h>
-#include <lib/geo/geo.h> // For CONSTANTS_*
+#include <lib/atmosphere/atmosphere.h>
 
 const char *const UavcanAirspeedBridge::NAME = "airspeed";
 
@@ -104,7 +104,7 @@ UavcanAirspeedBridge::ias_sub_cb(const
 	report.timestamp   		= hrt_absolute_time();
 	report.indicated_airspeed_m_s   = msg.indicated_airspeed;
 	report.true_airspeed_m_s   	= _last_tas_m_s;
-	report.air_temperature_celsius 	= _last_outside_air_temp_k + CONSTANTS_ABSOLUTE_NULL_CELSIUS;
+	report.air_temperature_celsius 	= _last_outside_air_temp_k + atmosphere::kAbsoluteNullCelsius;
 
 	publish(msg.getSrcNodeID().get(), &report);
 }

src/drivers/uavcan/sensors/baro.cpp

@@ -41,7 +41,7 @@
 
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/sensor_baro.h>
-#include <lib/geo/geo.h> // For CONSTANTS_*
+#include <lib/atmosphere/atmosphere.h>
 
 const char *const UavcanBarometerBridge::NAME = "baro";
 
@@ -78,10 +78,10 @@ void UavcanBarometerBridge::air_temperature_sub_cb(const
 
 	} else if (msg.static_temperature < 0) {
 		// handle previous incorrect temperature conversion to Kelvin where 273 was subtracted instead of added (https://github.com/PX4/PX4-Autopilot/pull/19061)
-		float temperature_c = msg.static_temperature - CONSTANTS_ABSOLUTE_NULL_CELSIUS;
+		float temperature_c = msg.static_temperature - atmosphere::kAbsoluteNullCelsius;
 
 		if (temperature_c > -40.f && temperature_c < 120.f) {
-			_last_temperature_kelvin = temperature_c - CONSTANTS_ABSOLUTE_NULL_CELSIUS;
+			_last_temperature_kelvin = temperature_c - atmosphere::kAbsoluteNullCelsius;
 		}
 	}
 }
@@ -119,7 +119,7 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const
 	sensor_baro.pressure = msg.static_pressure;
 
 	if (PX4_ISFINITE(_last_temperature_kelvin) && (_last_temperature_kelvin >= 0.f)) {
-		sensor_baro.temperature = _last_temperature_kelvin + CONSTANTS_ABSOLUTE_NULL_CELSIUS;
+		sensor_baro.temperature = _last_temperature_kelvin + atmosphere::kAbsoluteNullCelsius;
 
 	} else {
 		sensor_baro.temperature = NAN;

src/drivers/uavcan/sensors/battery.cpp

@@ -33,7 +33,7 @@
 
 #include "battery.hpp"
 
-#include <lib/geo/geo.h>
+#include <lib/atmosphere/atmosphere.h>
 #include <px4_defines.h>
 #include <px4_platform_common/log.h>
 
@@ -116,7 +116,7 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
 
 	_battery_status[instance].remaining = msg.state_of_charge_pct / 100.0f; // between 0 and 1
 	// _battery_status[instance].scale = msg.; // Power scaling factor, >= 1, or -1 if unknown
-	_battery_status[instance].temperature = msg.temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS; // Kelvin to Celsius
+	_battery_status[instance].temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius; // Kelvin to Celsius
 	// _battery_status[instance].cell_count = msg.;
 	_battery_status[instance].connected = true;
 	_battery_status[instance].source = msg.status_flags & uavcan::equipment::power::BatteryInfo::STATUS_FLAG_IN_USE;
@@ -239,7 +239,7 @@ UavcanBatteryBridge::filterData(const uavcan::ReceivedDataStructure<uavcan::equi
 
 	/* Override data that is expected to arrive from UAVCAN msg*/
 	_battery_status[instance] = _battery[instance]->getBatteryStatus();
-	_battery_status[instance].temperature = msg.temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS; // Kelvin to Celsius
+	_battery_status[instance].temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius; // Kelvin to Celsius
 	_battery_status[instance].serial_number = msg.model_instance_id;
 	_battery_status[instance].id = msg.getSrcNodeID().get(); // overwrite zeroed index from _battery
 

src/drivers/uavcan/sensors/differential_pressure.cpp

@@ -38,7 +38,7 @@
 #include "differential_pressure.hpp"
 
 #include <drivers/drv_hrt.h>
-#include <lib/geo/geo.h>
+#include <lib/atmosphere/atmosphere.h>
 #include <parameters/param.h>
 #include <systemlib/err.h>
 
@@ -71,7 +71,7 @@ void UavcanDifferentialPressureBridge::air_sub_cb(const
 	_device_id.devid_s.address = msg.getSrcNodeID().get() & 0xFF;
 
 	float diff_press_pa = msg.differential_pressure;
-	float temperature_c = msg.static_air_temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS;
+	float temperature_c = msg.static_air_temperature + atmosphere::kAbsoluteNullCelsius;
 
 	differential_pressure_s report{};
 	report.timestamp_sample = timestamp_sample;

src/drivers/uavcan/sensors/hygrometer.cpp

@@ -34,7 +34,7 @@
 #include "hygrometer.hpp"
 
 #include <drivers/drv_hrt.h>
-#include <lib/geo/geo.h>
+#include <lib/atmosphere/atmosphere.h>
 
 const char *const UavcanHygrometerBridge::NAME = "hygrometer_sensor";
 
@@ -65,7 +65,7 @@ void UavcanHygrometerBridge::hygro_sub_cb(const uavcan::ReceivedDataStructure<dr
 	sensor_hygrometer_s report{};
 	report.timestamp_sample = timestamp_sample;
 	report.device_id = 0; // TODO
-	report.temperature = msg.temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS;
+	report.temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius;
 	report.humidity = msg.humidity;
 	report.timestamp = hrt_absolute_time();
 

src/drivers/uavcannode/Publishers/BatteryInfo.hpp

@@ -39,6 +39,7 @@
 
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/battery_status.h>
+#include <lib/atmosphere/atmosphere.h>
 
 namespace uavcannode
 {
@@ -76,7 +77,7 @@ public:
 			uavcan::equipment::power::BatteryInfo battery_info{};
 			battery_info.voltage = battery.voltage_v;
 			battery_info.current = fabs(battery.current_a);
-			battery_info.temperature = battery.temperature - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // convert from C to K
+			battery_info.temperature = battery.temperature - atmosphere::kAbsoluteNullCelsius; // convert from C to K
 			battery_info.full_charge_capacity_wh = battery.capacity;
 			battery_info.remaining_capacity_wh = battery.remaining * battery.capacity;
 			battery_info.state_of_charge_pct = battery.remaining * 100;

src/drivers/uavcannode/Publishers/RawAirData.hpp

@@ -39,6 +39,7 @@
 
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/differential_pressure.h>
+#include <lib/atmosphere/atmosphere.h>
 
 namespace uavcannode
 {
@@ -78,8 +79,8 @@ public:
 			// raw_air_data.static_pressure =
 			raw_air_data.differential_pressure = diff_press.differential_pressure_pa;
 			// raw_air_data.static_pressure_sensor_temperature =
-			raw_air_data.differential_pressure_sensor_temperature = diff_press.temperature - CONSTANTS_ABSOLUTE_NULL_CELSIUS;
+			raw_air_data.differential_pressure_sensor_temperature = diff_press.temperature - atmosphere::kAbsoluteNullCelsius;
-			raw_air_data.static_air_temperature = diff_press.temperature - CONSTANTS_ABSOLUTE_NULL_CELSIUS;
+			raw_air_data.static_air_temperature = diff_press.temperature - atmosphere::kAbsoluteNullCelsius;
 			// raw_air_data.pitot_temperature
 			// raw_air_data.covariance
 			uavcan::Publisher<uavcan::equipment::air_data::RawAirData>::broadcast(raw_air_data);

src/drivers/uavcannode/Publishers/StaticTemperature.hpp

@@ -39,6 +39,7 @@
 
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/sensor_baro.h>
+#include <lib/atmosphere/atmosphere.h>
 
 namespace uavcannode
 {
@@ -74,7 +75,7 @@ public:
 
 		if ((hrt_elapsed_time(&_last_static_temperature_publish) > 1_s) && uORB::SubscriptionCallbackWorkItem::update(&baro)) {
 			uavcan::equipment::air_data::StaticTemperature static_temperature{};
-			static_temperature.static_temperature = baro.temperature - CONSTANTS_ABSOLUTE_NULL_CELSIUS;
+			static_temperature.static_temperature = baro.temperature - atmosphere::kAbsoluteNullCelsius;
 			uavcan::Publisher<uavcan::equipment::air_data::StaticTemperature>::broadcast(static_temperature);
 
 			// ensure callback is registered

src/lib/CMakeLists.txt

@@ -61,6 +61,7 @@ add_subdirectory(mixer_module EXCLUDE_FROM_ALL)
 add_subdirectory(motion_planning EXCLUDE_FROM_ALL)
 add_subdirectory(npfg EXCLUDE_FROM_ALL)
 add_subdirectory(perf EXCLUDE_FROM_ALL)
+add_subdirectory(fw_performance_model EXCLUDE_FROM_ALL)
 add_subdirectory(pid EXCLUDE_FROM_ALL)
 add_subdirectory(pid_design EXCLUDE_FROM_ALL)
 add_subdirectory(rate_control EXCLUDE_FROM_ALL)

src/lib/airspeed/airspeed.cpp

@@ -43,10 +43,10 @@
 #include "airspeed.h"
 
 #include <px4_platform_common/defines.h>
-#include <lib/geo/geo.h>
 #include <lib/atmosphere/atmosphere.h>
 
 using atmosphere::getDensityFromPressureAndTemp;
+using atmosphere::kAirDensitySeaLevelStandardAtmos;
 
 float calc_IAS_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, enum AIRSPEED_SENSOR_MODEL smodel,
 			 float tube_len, float tube_dia_mm, float differential_pressure, float pressure_ambient, float temperature_celsius)
@@ -157,7 +157,7 @@ float calc_IAS_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, enum AIRSPEED_
 	}
 
 	// computed airspeed without correction for inflow-speed at tip of pitot-tube
-	const float airspeed_uncorrected = sqrtf(2.0f * dp_tot / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
+	const float airspeed_uncorrected = sqrtf(2.0f * dp_tot / kAirDensitySeaLevelStandardAtmos);
 
 	// corrected airspeed
 	const float airspeed_corrected = airspeed_uncorrected + dv;
@@ -169,10 +169,10 @@ float calc_IAS_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, enum AIRSPEED_
 float calc_IAS(float differential_pressure)
 {
 	if (differential_pressure > 0.0f) {
-		return sqrtf((2.0f * differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
+		return sqrtf((2.0f * differential_pressure) / kAirDensitySeaLevelStandardAtmos);
 
 	} else {
-		return -sqrtf((2.0f * fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
+		return -sqrtf((2.0f * fabsf(differential_pressure)) / kAirDensitySeaLevelStandardAtmos);
 	}
 
 }
@@ -183,7 +183,7 @@ float calc_TAS_from_CAS(float speed_calibrated, float pressure_ambient, float te
 		temperature_celsius = 15.f; // ICAO Standard Atmosphere 15 degrees Celsius
 	}
 
-	return speed_calibrated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / getDensityFromPressureAndTemp(pressure_ambient,
+	return speed_calibrated * sqrtf(kAirDensitySeaLevelStandardAtmos / getDensityFromPressureAndTemp(pressure_ambient,
 					temperature_celsius));
 }
 
@@ -197,7 +197,7 @@ float calc_TAS(float total_pressure, float static_pressure, float temperature_ce
 	float density = getDensityFromPressureAndTemp(static_pressure, temperature_celsius);
 
 	if (density < 0.0001f || !PX4_ISFINITE(density)) {
-		density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
+		density = kAirDensitySeaLevelStandardAtmos;
 	}
 
 	float pressure_difference = total_pressure - static_pressure;
@@ -212,5 +212,5 @@ float calc_TAS(float total_pressure, float static_pressure, float temperature_ce
 
 float calc_calibrated_from_true_airspeed(float speed_true, float air_density)
 {
-	return speed_true * sqrtf(air_density / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
+	return speed_true * sqrtf(air_density / kAirDensitySeaLevelStandardAtmos);
 }

src/lib/atmosphere/atmosphere.cpp

@@ -41,19 +41,16 @@
 namespace atmosphere
 {
 
-static constexpr float kTempRefKelvin = 15.f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // temperature at base height in Kelvin
-static constexpr float kTempGradient = -6.5f / 1000.f; // temperature gradient in degrees per meter
-static constexpr float kPressRefSeaLevelPa = 101325.f; // pressure at sea level in Pa
 
 float getDensityFromPressureAndTemp(const float pressure_pa, const float temperature_celsius)
 {
-	return (pressure_pa / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS)));
+	return (pressure_pa / (kAirGasConstant * (temperature_celsius - kAbsoluteNullCelsius)));
 }
 float getPressureFromAltitude(const float altitude_m)
 {
 
 	return kPressRefSeaLevelPa * powf((altitude_m * kTempGradient + kTempRefKelvin) / kTempRefKelvin,
-					  -CONSTANTS_ONE_G / (kTempGradient * CONSTANTS_AIR_GAS_CONST));
+					  -CONSTANTS_ONE_G / (kTempGradient * kAirGasConstant));
 }
 float getAltitudeFromPressure(float pressure_pa, float pressure_sealevel_pa)
 {
@@ -70,7 +67,7 @@ float getAltitudeFromPressure(float pressure_pa, float pressure_sealevel_pa)
 	 * h = -------------------------------  + h1
 	 *                   a
 	 */
-	return (((powf(pressure_ratio, (-(kTempGradient * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * kTempRefKelvin) -
+	return (((powf(pressure_ratio, (-(kTempGradient * kAirGasConstant) / CONSTANTS_ONE_G))) * kTempRefKelvin) -
 		kTempRefKelvin) / kTempGradient;
 
 }

src/lib/atmosphere/atmosphere.h

@@ -44,6 +44,17 @@ namespace atmosphere
 
 // NOTE: We are currently only modelling the first layer of the US Standard Atmosphere 1976.
 // This means that the functions are only valid up to an altitude of 11km.
+static constexpr float kAirDensitySeaLevelStandardAtmos = 1.225f;				// kg/m^3
+
+// [kg/m^3] air density of standard atmosphere at 11000m above mean sea level (this is the upper limit for the standard
+// atmosphere model we are using, see atmosphere lib used)
+static constexpr float kAirDensityStandardAtmos11000Amsl = 0.3639;
+
+static constexpr float kAirGasConstant = 287.1f;					// J/(kg * K)
+static constexpr float kAbsoluteNullCelsius = -273.15f;				// °C
+static constexpr float kTempRefKelvin = 15.f - kAbsoluteNullCelsius; // temperature at base height in Kelvin
+static constexpr float kTempGradient = -6.5f / 1000.f; // temperature gradient in degrees per meter
+static constexpr float kPressRefSeaLevelPa = 101325.f; // pressure at sea level in Pa
 
 /**
 * Calculate air density given air pressure and temperature.

src/lib/drivers/smbus_sbs/SBS.hpp

@@ -47,8 +47,7 @@
 #include <lib/drivers/smbus/SMBus.hpp>
 #include <uORB/topics/battery_status.h>
 #include <px4_platform_common/param.h>
-#include <geo/geo.h>
+#include <lib/atmosphere/atmosphere.h>
-
 
 using namespace time_literals;
 
@@ -292,7 +291,7 @@ int SMBUS_SBS_BaseClass<T>::populate_smbus_data(battery_status_s &data)
 
 	// Read battery temperature and covert to Celsius.
 	ret |= _interface->read_word(BATT_SMBUS_TEMP, result);
-	data.temperature = ((float)result * 0.1f) + CONSTANTS_ABSOLUTE_NULL_CELSIUS;
+	data.temperature = ((float)result * 0.1f) + atmosphere::kAbsoluteNullCelsius;
 
 	return ret;
 

src/lib/fw_performance_model/CMakeLists.txt

@@ -0,0 +1,36 @@
+############################################################################
+#
+#   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(performance_model
+        PerformanceModel.cpp
+)

src/lib/fw_performance_model/PerformanceModel.cpp

@@ -0,0 +1,235 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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 PerformanceModel.cpp
+ * Performance model.
+ */
+
+#include <geo/geo.h>
+#include <px4_platform_common/events.h>
+#include "PerformanceModel.hpp"
+#include <lib/atmosphere/atmosphere.h>
+
+using namespace atmosphere;
+
+
+// [.] minimum ratio between the actual vehicle weight and the vehicle nominal weight (weight at which the performance limits are derived)
+static constexpr float kMinWeightRatio = 0.5f;
+
+// [.] maximum ratio between the actual vehicle weight and the vehicle nominal weight (weight at which the performance limits are derived)
+static constexpr float kMaxWeightRatio = 2.0f;
+
+// [m/s] climbrate defining the service ceiling, used to compensate max climbrate based on air density
+static constexpr float kClimbRateAtServiceCeiling = 0.5f;
+
+PerformanceModel::PerformanceModel(): ModuleParams(nullptr)
+{
+	updateParams();
+}
+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,
+						kMaxWeightRatio);
+	}
+
+	return weight_factor;
+}
+float PerformanceModel::getMaximumClimbRate(float air_density) const
+{
+	air_density = sanitiseAirDensity(air_density);
+	float climbrate_max = _param_fw_t_clmb_max.get();
+
+	const float service_ceiling = _param_service_ceiling.get();
+
+	if (service_ceiling > FLT_EPSILON) {
+		const float ceiling_pressure = getPressureFromAltitude(service_ceiling);
+		const float ceiling_density = getDensityFromPressureAndTemp(ceiling_pressure,
+					      getStandardTemperatureAtAltitude(service_ceiling));
+		const float climbrate_gradient = math::max((_param_fw_t_clmb_max.get() - kClimbRateAtServiceCeiling) /
+						 (kAirDensitySeaLevelStandardAtmos -
+						  ceiling_density), 0.0f);
+		const float delta_rho = air_density - kAirDensitySeaLevelStandardAtmos;
+		climbrate_max = math::constrain(_param_fw_t_clmb_max.get() + climbrate_gradient * delta_rho, kClimbRateAtServiceCeiling,
+						_param_fw_t_clmb_max.get());
+	}
+
+	return climbrate_max / getWeightRatio();
+}
+float PerformanceModel::getTrimThrottle(float throttle_min, float throttle_max, float airspeed_sp,
+					float air_density) const
+{
+	const float throttle_trim = getTrimThrottleForCalibratedAirspeed(airspeed_sp) * getAirDensityThrottleScale(
+					    air_density);
+	return math::constrain(throttle_trim, throttle_min, throttle_max);
+}
+
+float PerformanceModel::getAirDensityThrottleScale(float air_density) const
+{
+	air_density = sanitiseAirDensity(air_density);
+	float air_density_throttle_scale = 1.0f;
+
+	// scale throttle as a function of sqrt(rho0/rho)
+	const float eas2tas = sqrtf(kAirDensitySeaLevelStandardAtmos / air_density);
+	const float eas2tas_at_11km_amsl = sqrtf(kAirDensitySeaLevelStandardAtmos / kAirDensityStandardAtmos11000Amsl);
+	air_density_throttle_scale = math::constrain(eas2tas, 1.f, eas2tas_at_11km_amsl);
+
+	return air_density_throttle_scale;
+}
+float PerformanceModel::getTrimThrottleForCalibratedAirspeed(float calibrated_airspeed_sp) const
+{
+	float throttle_trim =
+		_param_fw_thr_trim.get(); // throttle required for level flight at trim airspeed, at sea level (standard atmosphere)
+
+	// Drag modelling (parasite drag): calculate mapping airspeed-->throttle, assuming a linear relation with different gradients
+// above and below trim. This is tunable thorugh FW_THR_ASPD_MIN and FW_THR_ASPD_MAX.
+	const float slope_below_trim = (_param_fw_thr_trim.get() - _param_fw_thr_aspd_min.get()) /
+				       (_param_fw_airspd_trim.get() - _param_fw_airspd_min.get());
+	const float slope_above_trim = (_param_fw_thr_aspd_max.get() - _param_fw_thr_trim.get()) /
+				       (_param_fw_airspd_max.get() - _param_fw_airspd_trim.get());
+
+	if (PX4_ISFINITE(calibrated_airspeed_sp) && PX4_ISFINITE(slope_below_trim) && _param_fw_thr_aspd_min.get() > FLT_EPSILON
+	    && calibrated_airspeed_sp < _param_fw_airspd_trim.get()) {
+		throttle_trim = _param_fw_thr_trim.get() - slope_below_trim * (_param_fw_airspd_trim.get() - calibrated_airspeed_sp);
+
+	} else if (PX4_ISFINITE(calibrated_airspeed_sp) && PX4_ISFINITE(slope_above_trim)
+		   && _param_fw_thr_aspd_max.get() > FLT_EPSILON
+		   && calibrated_airspeed_sp > _param_fw_airspd_trim.get()) {
+		throttle_trim = _param_fw_thr_trim.get() + slope_above_trim * (calibrated_airspeed_sp - _param_fw_airspd_trim.get());
+	}
+
+	return throttle_trim;
+}
+float PerformanceModel::getMinimumSinkRate(float air_density) const
+{
+	air_density = sanitiseAirDensity(air_density);
+	return _param_fw_t_sink_min.get() * sqrtf(getWeightRatio() * kAirDensitySeaLevelStandardAtmos / air_density);
+}
+float PerformanceModel::getCalibratedTrimAirspeed() const
+{
+	return math::constrain(_param_fw_airspd_trim.get() * sqrtf(getWeightRatio()), _param_fw_airspd_min.get(),
+			       _param_fw_airspd_max.get());
+}
+float PerformanceModel::getMinimumCalibratedAirspeed(float load_factor) const
+{
+
+	load_factor = math::max(load_factor, FLT_EPSILON);
+	return _param_fw_airspd_min.get() * sqrtf(getWeightRatio() * load_factor);
+}
+
+float PerformanceModel::getCalibratedStallAirspeed(float load_factor) const
+{
+	load_factor = math::max(load_factor, FLT_EPSILON);
+	return _param_fw_airspd_stall.get() * sqrtf(getWeightRatio() * load_factor);
+}
+
+float PerformanceModel::getMaximumCalibratedAirspeed() const
+{
+	return _param_fw_airspd_max.get();
+}
+bool PerformanceModel::runSanityChecks() const
+{
+	bool ret = true;
+
+	// sanity check parameters
+	if (_param_fw_airspd_max.get() < _param_fw_airspd_min.get()) {
+		/* EVENT
+		 * @description
+		 * - <param>FW_AIRSPD_MAX</param>: {1:.1}
+		 * - <param>FW_AIRSPD_MIN</param>: {2:.1}
+		 */
+		events::send<float, float>(events::ID("fixedwing_position_control_conf_invalid_airspeed"), events::Log::Error,
+					   "Invalid configuration: Airspeed max smaller than min",
+					   _param_fw_airspd_max.get(), _param_fw_airspd_min.get());
+		ret = false;
+	}
+
+	if (_param_fw_airspd_max.get() < 5.0f || _param_fw_airspd_min.get() > 100.0f) {
+		/* EVENT
+		 * @description
+		 * - <param>FW_AIRSPD_MAX</param>: {1:.1}
+		 * - <param>FW_AIRSPD_MIN</param>: {2:.1}
+		 */
+		events::send<float, float>(events::ID("fixedwing_position_control_conf_invalid_airspeed_bounds"), events::Log::Error,
+					   "Invalid configuration: Airspeed max \\< 5 m/s or min \\> 100 m/s",
+					   _param_fw_airspd_max.get(), _param_fw_airspd_min.get());
+		ret = false;
+	}
+
+	if (_param_fw_airspd_trim.get() < _param_fw_airspd_min.get() ||
+	    _param_fw_airspd_trim.get() > _param_fw_airspd_max.get()) {
+		/* EVENT
+		 * @description
+		 * - <param>FW_AIRSPD_MAX</param>: {1:.1}
+		 * - <param>FW_AIRSPD_MIN</param>: {2:.1}
+		 * - <param>FW_AIRSPD_TRIM</param>: {3:.1}
+		 */
+		events::send<float, float, float>(events::ID("fixedwing_position_control_conf_invalid_trim_bounds"),
+						  events::Log::Error,
+						  "Invalid configuration: Airspeed trim out of min or max bounds",
+						  _param_fw_airspd_max.get(), _param_fw_airspd_min.get(), _param_fw_airspd_trim.get());
+		ret = false;
+	}
+
+	if (_param_fw_airspd_stall.get() > _param_fw_airspd_min.get()) {
+		/* EVENT
+		 * @description
+		 * - <param>FW_AIRSPD_MIN</param>: {1:.1}
+		 * - <param>FW_AIRSPD_STALL</param>: {2:.1}
+		 */
+		events::send<float, float>(events::ID("fixedwing_position_control_conf_invalid_stall"), events::Log::Error,
+					   "Invalid configuration: FW_AIRSPD_STALL higher FW_AIRSPD_MIN",
+					   _param_fw_airspd_min.get(), _param_fw_airspd_stall.get());
+		ret = false;
+	}
+
+	return ret;
+
+}
+void PerformanceModel::	updateParameters()
+{
+	updateParams();
+}
+
+float PerformanceModel::sanitiseAirDensity(float air_density)
+{
+	if (!PX4_ISFINITE(air_density)) {
+		air_density = kAirDensitySeaLevelStandardAtmos;
+	}
+
+	return math::max(air_density, kAirDensityStandardAtmos11000Amsl);
+}

src/lib/fw_performance_model/PerformanceModel.hpp

@@ -0,0 +1,151 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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 PerformanceModel.hpp
+ * Performance model.
+ */
+
+#include <px4_platform_common/module_params.h>
+
+#ifndef PX4_SRC_MODULES_FW_POS_CONTROL_PERFORMANCEMODEL_H_
+#define PX4_SRC_MODULES_FW_POS_CONTROL_PERFORMANCEMODEL_H_
+
+class PerformanceModel : public ModuleParams
+{
+public:
+	PerformanceModel();
+	~PerformanceModel() = default;
+
+	void updateParameters();
+
+	/**
+	 * Get the maximum climb rate (true airspeed) expected for current air density and weight.
+	 * @param air_density in kg/m^3
+	 * @return maximum climb rate in m/s
+	 */
+	float getMaximumClimbRate(float air_density) const;
+
+	/**
+	 * Get the minimum sink rate (true airspeed) expected for current air density and weight.
+	 * @param air_density in kg/m^3
+	 * @return minimum sink rate in m/s
+	 */
+	float getMinimumSinkRate(float air_density) const;
+
+	/**
+	 * Get the ration of actual weight to base weight
+	 * @return weight ratio
+	 */
+	float getWeightRatio() const;
+
+	/**
+	 * 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]
+	 * @param throttle_max maximum throttle in range [0,1]
+	 * @param airspeed_sp calibrated airspeed setpoint in m/s
+	 * @param air_density air density in kg/m^3
+	 * @return trim throttle in range [0,1]
+	 */
+	float getTrimThrottle(float throttle_min, float throttle_max, float airspeed_sp, float air_density) const;
+
+	/**
+	 * Get the throttle scale factor for the current air density.
+	 * @param air_density in kg/m^3
+	 * @return throttle scale factor for air density
+	 */
+	float getAirDensityThrottleScale(float air_density) const;
+
+	/**
+	 * Get the trim airspeed compensated for weight.
+	 * @return calibrated trim airspeed in m/s
+	 */
+	float getCalibratedTrimAirspeed() const;
+
+	/**
+	 * Get the minimum airspeed compensated for weight and load factor due to bank angle.
+	 * @param load_factor due to banking
+	 * @return calibrated minimum airspeed in m/s
+	 */
+	float getMinimumCalibratedAirspeed(float load_factor = 1.0f) const;
+
+	/**
+	 * Get the maximum airspeed.
+	 * @return calibrated maximum airspeed in m/s
+	 */
+	float getMaximumCalibratedAirspeed() const;
+
+	/**
+	 * get the stall airspeed compensated for load factor due to bank angle.
+	 * @param load_factor load factor due to banking
+	 * @return calibrated stall airspeed in m/s
+	 */
+	float getCalibratedStallAirspeed(float load_factor) const;
+
+	/**
+	 * Run some checks on parameters and detect unfeasible combinations.
+	 * @return true if all checks pass
+	 */
+	bool runSanityChecks() const;
+
+private:
+	DEFINE_PARAMETERS(
+		(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_AIRSPD_STALL>) _param_fw_airspd_stall,
+		(ParamFloat<px4::params::FW_T_CLMB_MAX>) _param_fw_t_clmb_max,
+		(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::FW_SERVICE_CEIL>) _param_service_ceiling,
+		(ParamFloat<px4::params::FW_THR_TRIM>) _param_fw_thr_trim,
+		(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_ASPD_MIN>) _param_fw_thr_aspd_min,
+		(ParamFloat<px4::params::FW_THR_ASPD_MAX>) _param_fw_thr_aspd_max)
+
+	/**
+	 * Get the sea level trim throttle for a given calibrated airspeed setpoint.
+	 * @param calibrated_airspeed_sp [m/s] calibrated
+	 * @return trim throttle [0, 1] at sea level
+	 */
+	float getTrimThrottleForCalibratedAirspeed(float calibrated_airspeed_sp) const;
+
+
+	static float sanitiseAirDensity(float air_density);
+};
+
+#endif //PX4_SRC_MODULES_FW_POS_CONTROL_PERFORMANCEMODEL_H_

src/lib/fw_performance_model/performance_model_params.c

@@ -0,0 +1,214 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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.
+ *
+ ****************************************************************************/
+
+/**
+ * Trim throttle
+ *
+ * Required throttle (at sea level, standard atmosphere) for level flight at FW_AIRSPD_TRIM
+ *
+ * @unit norm
+ * @min 0.0
+ * @max 1.0
+ * @decimal 2
+ * @increment 0.01
+ * @group FW Performance
+ */
+PARAM_DEFINE_FLOAT(FW_THR_TRIM, 0.6f);
+
+/**
+ * Throttle at min airspeed
+ *
+ * Required throttle (at sea level, standard atmosphere) for level flight at minimum airspeed FW_AIRSPD_MIN
+ *
+ * Set to 0 to disable mapping of airspeed to trim throttle below FW_AIRSPD_TRIM.
+ *
+ * @min 0
+ * @max 1
+ * @decimal 2
+ * @increment 0.01
+ * @group FW Performance
+ */
+PARAM_DEFINE_FLOAT(FW_THR_ASPD_MIN, 0.f);
+
+/**
+ * Throttle at max airspeed
+ *
+ * Required throttle (at sea level, standard atmosphere) for level flight at maximum airspeed FW_AIRSPD_MAX
+ *
+ * Set to 0 to disable mapping of airspeed to trim throttle.
+ *
+ * @min 0
+ * @max 1
+ * @decimal 2
+ * @increment 0.01
+ * @group FW Performance
+ */
+PARAM_DEFINE_FLOAT(FW_THR_ASPD_MAX, 0.f);
+
+
+/**
+ * Service ceiling
+ *
+ * Altitude in standard atmosphere at which the vehicle in normal configuration (WEIGHT_BASE) is still able to achieve a maximum climb rate of
+ * 0.5m/s at maximum throttle (FW_THR_MAX). Used to compensate for air density in FW_T_CLMB_MAX.
+ * Set negative to disable.
+ *
+ * @min -1.0
+ * @unit m
+ * @decimal 0
+ * @increment 1.0
+ * @group FW Performance
+ */
+PARAM_DEFINE_FLOAT(FW_SERVICE_CEIL, -1.0f);
+
+/**
+ * Vehicle base weight.
+ *
+ * This is the weight of the vehicle at which it's performance limits were derived. A zero or negative value
+ * disables trim throttle and minimum airspeed compensation based on weight.
+ *
+ * @unit kg
+ * @decimal 1
+ * @increment 0.5
+ * @group FW Performance
+ */
+PARAM_DEFINE_FLOAT(WEIGHT_BASE, -1.0f);
+
+/**
+ * Vehicle gross weight.
+ *
+ * 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.
+ *
+ * @unit kg
+ * @decimal 1
+ * @increment 0.1
+ * @group FW Performance
+ */
+PARAM_DEFINE_FLOAT(WEIGHT_GROSS, -1.0f);
+
+/**
+ * Maximum climb rate
+ *
+ * This is the maximum calibrated climb rate that the aircraft can achieve with
+ * the throttle set to FW_THR_MAX and the airspeed set to the
+ * trim value. For electric aircraft make sure this number can be
+ * achieved towards the end of flight when the battery voltage has reduced.
+ *
+ * @unit m/s
+ * @min 1.0
+ * @max 15.0
+ * @decimal 1
+ * @increment 0.5
+ * @group FW Performance
+ */
+PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
+
+/**
+ * Minimum descent rate
+ *
+ * This is the minimum calibrated sink rate of the aircraft with the throttle
+ * set to THR_MIN and flown at the same airspeed as used
+ * to measure FW_T_CLMB_MAX.
+ *
+ * @unit m/s
+ * @min 1.0
+ * @max 5.0
+ * @decimal 1
+ * @increment 0.5
+ * @group FW Performance
+ */
+PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
+
+/**
+ * Trim (Cruise) Airspeed
+ *
+ * The trim CAS (calibrated airspeed) of the vehicle. If an airspeed controller is active,
+ * this is the default airspeed setpoint that the controller will try to achieve.
+ * This value corresponds to the trim airspeed with the default load factor (level flight, default weight).
+ *
+ * @unit m/s
+ * @min 0.5
+ * @decimal 1
+ * @increment 0.5
+ * @group FW Performance
+ */
+PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f);
+
+/**
+ * Stall Airspeed (CAS)
+ *
+ * The stall airspeed (calibrated airspeed) of the vehicle.
+ * It is used for airspeed sensor failure detection and for the control
+ * surface scaling airspeed limits.
+ *
+ * @unit m/s
+ * @min 0.5
+ * @decimal 1
+ * @increment 0.5
+ * @group FW Performance
+ */
+PARAM_DEFINE_FLOAT(FW_AIRSPD_STALL, 7.0f);
+
+/**
+ * Minimum Airspeed (CAS)
+ *
+ * The minimal airspeed (calibrated airspeed) the user is able to command.
+ * Further, if the airspeed falls below this value, the TECS controller will try to
+ * increase airspeed more aggressively.
+ * Has to be set according to the vehicle's stall speed (which should be set in FW_AIRSPD_STALL),
+ * with some margin between the stall speed and minimum airspeed.
+ * This value corresponds to the desired minimum speed with the default load factor (level flight, default weight),
+ * and is automatically adpated to the current load factor (calculated from roll setpoint and WEIGHT_GROSS/WEIGHT_BASE).
+ *
+ * @unit m/s
+ * @min 0.5
+ * @decimal 1
+ * @increment 0.5
+ * @group FW Performance
+ */
+PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f);
+
+/**
+ * Maximum Airspeed (CAS)
+ *
+ * The maximal airspeed (calibrated airspeed) the user is able to command.
+ *
+ * @unit m/s
+ * @min 0.5
+ * @decimal 1
+ * @increment 0.5
+ * @group FW Performance
+ */
+PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f);

src/lib/geo/geo.h

@@ -54,19 +54,8 @@
 #include <lib/matrix/matrix/math.hpp>
 
 static constexpr float CONSTANTS_ONE_G = 9.80665f;						// m/s^2
-
-static constexpr float CONSTANTS_STD_PRESSURE_PA = 101325.0f;					// pascals (Pa)
-static constexpr float CONSTANTS_STD_PRESSURE_KPA = CONSTANTS_STD_PRESSURE_PA / 1000.0f;	// kilopascals (kPa)
-static constexpr float CONSTANTS_STD_PRESSURE_MBAR = CONSTANTS_STD_PRESSURE_PA /
-		100.0f;	// Millibar (mbar) (1 mbar = 100 Pa)
-
-static constexpr float CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C = 1.225f;				// kg/m^3
-static constexpr float CONSTANTS_AIR_GAS_CONST = 287.1f;					// J/(kg * K)
-static constexpr float CONSTANTS_ABSOLUTE_NULL_CELSIUS = -273.15f;				// °C
-
 static constexpr double CONSTANTS_RADIUS_OF_EARTH = 6371000;					// meters (m)
 static constexpr float  CONSTANTS_RADIUS_OF_EARTH_F = CONSTANTS_RADIUS_OF_EARTH;		// meters (m)
-
 static constexpr float CONSTANTS_EARTH_SPIN_RATE = 7.2921150e-5f;				// radians/second (rad/s)
 
 

src/lib/parameters/px4params/srcparser.py

@@ -357,7 +357,7 @@ class SourceParser(object):
                                 'bit/s', 'B/s',
                                 'deg', 'deg*1e7', 'deg/s',
                                 'celcius', 'gauss', 'gauss/s', 'gauss^2',
-                                'hPa', 'kg', 'kg/m^2', 'kg m^2',
+                                'hPa', 'kg', 'kg/m^2', 'kg m^2', 'kg/m^3',
                                 'mm', 'm', 'm/s', 'm^2', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', '1/s/sqrt(Hz)', 'm/s/rad',
                                 'Ohm', 'V', 'A',
                                 'us', 'ms', 's',

src/lib/tecs/TECS.cpp

@@ -592,11 +592,11 @@ float TECSControl::_calcThrottleControlOutput(const STERateLimit &limit, const C
 
 	if (ste_rate.setpoint >= FLT_EPSILON) {
 		// throttle is between trim and maximum
-		throttle_predicted = param.throttle_trim_adjusted + ste_rate.setpoint * throttle_above_trim_per_ste_rate;
+		throttle_predicted = param.throttle_trim + ste_rate.setpoint * throttle_above_trim_per_ste_rate;
 
 	} else {
 		// throttle is between trim and minimum
-		throttle_predicted = param.throttle_trim_adjusted - ste_rate.setpoint * throttle_below_trim_per_ste_rate;
+		throttle_predicted = param.throttle_trim - ste_rate.setpoint * throttle_below_trim_per_ste_rate;
 
 	}
 
@@ -661,7 +661,7 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo
 
 void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed,
 		  float eas_to_tas, float throttle_min, float throttle_setpoint_max,
-		  float throttle_trim, float throttle_trim_adjusted, float pitch_limit_min, float pitch_limit_max, float target_climbrate,
+		  float throttle_trim, float pitch_limit_min, float pitch_limit_max, float target_climbrate,
 		  float target_sinkrate, const float speed_deriv_forward, float hgt_rate, float hgt_rate_sp)
 {
 
@@ -678,7 +678,6 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 	_control_param.pitch_max = pitch_limit_max;
 	_control_param.pitch_min = pitch_limit_min;
 	_control_param.throttle_trim = throttle_trim;
-	_control_param.throttle_trim_adjusted = throttle_trim_adjusted;
 	_control_param.throttle_max = throttle_setpoint_max;
 	_control_param.throttle_min = throttle_min;
 

src/lib/tecs/TECS.hpp

@@ -198,11 +198,10 @@ public:
 		float max_climb_rate;			///< Climb rate produced by max allowed throttle [m/s].
 		float vert_accel_limit;			///< Magnitude of the maximum vertical acceleration allowed [m/s²].
 		float equivalent_airspeed_trim;		///< Equivalent cruise airspeed for airspeed less mode [m/s].
-		float tas_min;				///< True airpeed demand lower limit [m/s].
+		float tas_min;				///< True airspeed demand lower limit [m/s].
 		float pitch_max;			///< Maximum pitch angle allowed in [rad].
 		float pitch_min;			///< Minimal pitch angle allowed in [rad].
-		float throttle_trim;			///< Normalized throttle required to fly level at trim airspeed and sea level
+		float throttle_trim;		///< Normalized throttle required to fly level at calibrated airspeed setpoint [0,1]
-		float throttle_trim_adjusted;		///< Trim throttle adjusted for airspeed, load factor and air density
 		float throttle_max;			///< Normalized throttle upper limit.
 		float throttle_min;			///< Normalized throttle lower limit.
 
@@ -577,7 +576,7 @@ public:
 	 */
 	void update(float pitch, float altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed,
 		    float eas_to_tas, float throttle_min, float throttle_setpoint_max,
-		    float throttle_trim, float throttle_trim_adjusted, float pitch_limit_min, float pitch_limit_max, float target_climbrate,
+		    float throttle_trim, float pitch_limit_min, float pitch_limit_max, float target_climbrate,
 		    float target_sinkrate, float speed_deriv_forward, float hgt_rate, float hgt_rate_sp = NAN);
 
 	/**
@@ -690,7 +689,6 @@ private:
 		.pitch_max = 5.0f,
 		.pitch_min = -5.0f,
 		.throttle_trim = 0.0f,
-		.throttle_trim_adjusted = 0.f,
 		.throttle_max = 1.0f,
 		.throttle_min = 0.1f,
 		.altitude_error_gain = 0.2f,

src/modules/commander/CMakeLists.txt

@@ -75,6 +75,7 @@ px4_add_module(
 		MulticopterThrowLaunch
 		sensor_calibration
 		world_magnetic_model
+        atmosphere
 	)
 
 px4_add_unit_gtest(SRC mag_calibration_test.cpp LINKLIBS modules__commander)

src/modules/commander/baro_calibration.cpp

@@ -48,7 +48,7 @@
 #include <px4_platform_common/time.h>
 #include <drivers/drv_hrt.h>
 #include <matrix/math.hpp>
-#include <lib/geo/geo.h>
+#include <lib/atmosphere/atmosphere.h>
 #include <lib/sensor_calibration/Barometer.hpp>
 #include <lib/sensor_calibration/Utilities.hpp>
 #include <lib/systemlib/mavlink_log.h>
@@ -60,37 +60,12 @@
 
 using namespace matrix;
 using namespace time_literals;
+using namespace atmosphere;
 
 static constexpr char sensor_name[] {"baro"};
 
 static constexpr int MAX_SENSOR_COUNT = 4;
 
-static float PressureToAltitude(float pressure_pa, float temperature)
-{
-	// calculate altitude using the hypsometric equation
-	static constexpr float T1 = 15.f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // temperature at base height in Kelvin
-	static constexpr float a = -6.5f / 1000.f; // temperature gradient in degrees per metre
-
-	// current pressure at MSL in kPa (QNH in hPa)
-	const float p1 = 1013.25f * 0.1f;
-
-	// measured pressure in kPa
-	const float p = pressure_pa * 0.001f;
-
-	/*
-	 * Solve:
-	 *
-	 *     /        -(aR / g)     \
-	 *    | (p / p1)          . T1 | - T1
-	 *     \                      /
-	 * h = -------------------------------  + h1
-	 *                   a
-	 */
-	float altitude = (((powf((p / p1), (-(a * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * T1) - T1) / a;
-
-	return altitude;
-}
-
 int do_baro_calibration(orb_advert_t *mavlink_log_pub)
 {
 	calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
@@ -172,7 +147,7 @@ int do_baro_calibration(orb_advert_t *mavlink_log_pub)
 			const float pressure_pa = data_sum[instance] / data_sum_count[instance];
 			const float temperature = temperature_sum[instance] / data_sum_count[instance];
 
-			float pressure_altitude = PressureToAltitude(pressure_pa, temperature);
+			float pressure_altitude = getAltitudeFromPressure(pressure_pa, temperature);
 
 			// Use GPS altitude as a reference to compute the baro bias measurement
 			const float baro_bias = pressure_altitude - gps_altitude;
@@ -189,7 +164,7 @@ int do_baro_calibration(orb_advert_t *mavlink_log_pub)
 			// perform a binary search
 			while (front <= last) {
 				middle = front + (last - front) / 2;
-				float altitude_calibrated = PressureToAltitude(pressure_pa - middle, temperature);
+				float altitude_calibrated = getAltitudeFromPressure(pressure_pa - middle, temperature);
 
 				if (altitude_calibrated > altitude + 0.1f) {
 					last = middle;

src/modules/ekf2/EKF/drag_fusion.cpp

@@ -41,6 +41,7 @@
 #include <ekf_derivation/generated/compute_drag_y_innov_var_and_k.h>
 
 #include <mathlib/mathlib.h>
+#include <lib/atmosphere/atmosphere.h>
 
 void Ekf::controlDragFusion(const imuSample &imu_delayed)
 {
@@ -67,7 +68,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
 
 	// correct rotor momentum drag for increase in required rotor mass flow with altitude
 	// obtained from momentum disc theory
-	const float mcoef_corrrected = fmaxf(_params.mcoef * sqrtf(rho / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C), 0.f);
+	const float mcoef_corrrected = fmaxf(_params.mcoef * sqrtf(rho / atmosphere::kAirDensitySeaLevelStandardAtmos), 0.f);
 
 	// drag model parameters
 	const bool using_bcoef_x = _params.bcoef_x > 1.0f;

src/modules/ekf2/EKF/estimator_interface.h

@@ -71,7 +71,7 @@
 # include "sensor_range_finder.hpp"
 #endif // CONFIG_EKF2_RANGE_FINDER
 
-#include <lib/geo/geo.h>
+#include <lib/atmosphere/atmosphere.h>
 #include <matrix/math.hpp>
 #include <mathlib/mathlib.h>
 #include <mathlib/math/filter/AlphaFilter.hpp>
@@ -374,7 +374,7 @@ protected:
 	float _flow_max_distance{10.f};	///< maximum distance that the optical flow sensor can operate at (m)
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
-	float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C};		// air density (kg/m**3)
+	float _air_density{atmosphere::kAirDensitySeaLevelStandardAtmos};		// air density (kg/m**3)
 
 	bool _imu_updated{false};      // true if the ekf should update (completed downsampling process)
 	bool _initialised{false};      // true if the ekf interface instance (data buffering) is initialized

src/modules/ekf2/test/test_EKF_drag_fusion.cpp

@@ -39,6 +39,7 @@
 #include "EKF/ekf.h"
 #include "sensor_simulator/sensor_simulator.h"
 #include "sensor_simulator/ekf_wrapper.h"
+#include <lib/atmosphere/atmosphere.h>
 
 class EkfDragFusionTest : public ::testing::Test
 {
@@ -179,7 +180,7 @@ TEST_F(EkfDragFusionTest, testForwardBluffBodyDrag)
 
 	Vector2f predicted_accel(CONSTANTS_ONE_G * sinf(pitch), 0.0f);
 	const float airspeed = sqrtf((2.0f * bcoef_x * predicted_accel.length()) /
-				     CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
+				     atmosphere::kAirDensitySeaLevelStandardAtmos);
 	Vector2f wind_speed(-airspeed, 0.0f);
 
 	// The magnitude of error perpendicular to wind is equivalent to the error in the direction of wind
@@ -219,7 +220,7 @@ TEST_F(EkfDragFusionTest, testLateralBluffBodyDrag)
 
 	Vector2f predicted_accel(0.0f, - CONSTANTS_ONE_G * sinf(roll));
 	const float airspeed = sqrtf((2.0f * bcoef_y * predicted_accel.length()) /
-				     CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
+				     atmosphere::kAirDensitySeaLevelStandardAtmos);
 	Vector2f wind_speed(0.0f, -airspeed);
 
 	// The magnitude of error perpendicular to wind is equivalent to the error in the of wind
@@ -259,7 +260,7 @@ TEST_F(EkfDragFusionTest, testDiagonalBluffBodyDrag)
 
 	Vector2f predicted_accel = quat_sim.rotateVectorInverse(Vector3f(0.f, 0.f, -CONSTANTS_ONE_G)).xy();
 	const float airspeed = sqrtf((2.0f * bcoef_y * predicted_accel.norm()) /
-				     CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
+				     atmosphere::kAirDensitySeaLevelStandardAtmos);
 	Vector2f wind_speed(airspeed * predicted_accel / predicted_accel.norm());
 
 	// The magnitude of error perpendicular to wind is equivalent to the error in the of wind

src/modules/fw_pos_control/CMakeLists.txt

@@ -41,6 +41,7 @@ set(POSCONTROL_DEPENDENCIES
 	SlewRate
 	tecs
 	motion_planning
+    performance_model
 )
 
 if(CONFIG_FIGURE_OF_EIGHT)

src/modules/fw_pos_control/FixedwingPositionControl.cpp

@@ -80,7 +80,7 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
 	/* fetch initial parameter values */
 	parameters_update();
 
-	_airspeed_slew_rate_controller.setForcedValue(_param_fw_airspd_trim.get());
+	_airspeed_slew_rate_controller.setForcedValue(_performance_model.getCalibratedTrimAirspeed());
 }
 
 FixedwingPositionControl::~FixedwingPositionControl()
@@ -99,11 +99,13 @@ FixedwingPositionControl::init()
 	return true;
 }
 
-int
+void
 FixedwingPositionControl::parameters_update()
 {
 	updateParams();
 
+	_performance_model.updateParameters();
+
 	// VTOL parameter VT_ARSP_TRANS
 	if (_param_handle_airspeed_trans != PARAM_INVALID) {
 		param_get(_param_handle_airspeed_trans, &_param_airspeed_trans);
@@ -126,12 +128,12 @@ FixedwingPositionControl::parameters_update()
 	_npfg.setPeriodSafetyFactor(_param_npfg_period_safety_factor.get());
 
 	// TECS parameters
-	_tecs.set_max_climb_rate(_param_fw_t_clmb_max.get());
+	_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(_param_fw_t_sink_min.get());
+	_tecs.set_min_sink_rate(_performance_model.getMinimumSinkRate(_air_density));
 	_tecs.set_speed_weight(_param_fw_t_spdweight.get());
-	_tecs.set_equivalent_airspeed_trim(_param_fw_airspd_trim.get());
+	_tecs.set_equivalent_airspeed_trim(_performance_model.getCalibratedTrimAirspeed());
-	_tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get());
+	_tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed());
 	_tecs.set_throttle_damp(_param_fw_t_thr_damp.get());
 	_tecs.set_integrator_gain_throttle(_param_fw_t_I_gain_thr.get());
 	_tecs.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get());
@@ -148,61 +150,7 @@ FixedwingPositionControl::parameters_update()
 	_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());
 
-	int check_ret = PX4_OK;
+	_performance_model.runSanityChecks();
-
-	// sanity check parameters
-	if (_param_fw_airspd_max.get() < _param_fw_airspd_min.get()) {
-		/* EVENT
-		 * @description
-		 * - <param>FW_AIRSPD_MAX</param>: {1:.1}
-		 * - <param>FW_AIRSPD_MIN</param>: {2:.1}
-		 */
-		events::send<float, float>(events::ID("fixedwing_position_control_conf_invalid_airspeed"), events::Log::Error,
-					   "Invalid configuration: Airspeed max smaller than min",
-					   _param_fw_airspd_max.get(), _param_fw_airspd_min.get());
-		check_ret = PX4_ERROR;
-	}
-
-	if (_param_fw_airspd_max.get() < 5.0f || _param_fw_airspd_min.get() > 100.0f) {
-		/* EVENT
-		 * @description
-		 * - <param>FW_AIRSPD_MAX</param>: {1:.1}
-		 * - <param>FW_AIRSPD_MIN</param>: {2:.1}
-		 */
-		events::send<float, float>(events::ID("fixedwing_position_control_conf_invalid_airspeed_bounds"), events::Log::Error,
-					   "Invalid configuration: Airspeed max \\< 5 m/s or min \\> 100 m/s",
-					   _param_fw_airspd_max.get(), _param_fw_airspd_min.get());
-		check_ret = PX4_ERROR;
-	}
-
-	if (_param_fw_airspd_trim.get() < _param_fw_airspd_min.get() ||
-	    _param_fw_airspd_trim.get() > _param_fw_airspd_max.get()) {
-		/* EVENT
-		 * @description
-		 * - <param>FW_AIRSPD_MAX</param>: {1:.1}
-		 * - <param>FW_AIRSPD_MIN</param>: {2:.1}
-		 * - <param>FW_AIRSPD_TRIM</param>: {3:.1}
-		 */
-		events::send<float, float, float>(events::ID("fixedwing_position_control_conf_invalid_trim_bounds"),
-						  events::Log::Error,
-						  "Invalid configuration: Airspeed trim out of min or max bounds",
-						  _param_fw_airspd_max.get(), _param_fw_airspd_min.get(), _param_fw_airspd_trim.get());
-		check_ret = PX4_ERROR;
-	}
-
-	if (_param_fw_airspd_stall.get() > _param_fw_airspd_min.get()) {
-		/* EVENT
-		 * @description
-		 * - <param>FW_AIRSPD_MIN</param>: {1:.1}
-		 * - <param>FW_AIRSPD_STALL</param>: {2:.1}
-		 */
-		events::send<float, float>(events::ID("fixedwing_position_control_conf_invalid_stall"), events::Log::Error,
-					   "Invalid configuration: FW_AIRSPD_STALL higher FW_AIRSPD_MIN",
-					   _param_fw_airspd_min.get(), _param_fw_airspd_stall.get());
-		check_ret = PX4_ERROR;
-	}
-
-	return check_ret;
 }
 
 void
@@ -378,24 +326,26 @@ FixedwingPositionControl::vehicle_attitude_poll()
 		_body_velocity_x = body_velocity(0);
 
 		// load factor due to banking
-		const float load_factor = 1.f / cosf(euler_angles(0));
+		_tecs.set_load_factor(getLoadFactor());
-		_tecs.set_load_factor(load_factor);
 	}
 }
 
 float
 FixedwingPositionControl::get_manual_airspeed_setpoint()
 {
-	float altctrl_airspeed = _param_fw_airspd_trim.get();
+
+	float altctrl_airspeed = _performance_model.getCalibratedTrimAirspeed();
 
 	if (_param_fw_pos_stk_conf.get() & STICK_CONFIG_ENABLE_AIRSPEED_SP_MANUAL_BIT) {
 		// neutral throttle corresponds to trim airspeed
 		return math::interpolateNXY(_manual_control_setpoint_for_airspeed,
 		{-1.f, 0.f, 1.f},
-		{_param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), _param_fw_airspd_max.get()});
+		{_performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), altctrl_airspeed, _performance_model.getMaximumCalibratedAirspeed()});
 
 	} else if (PX4_ISFINITE(_commanded_manual_airspeed_setpoint)) {
-		altctrl_airspeed = _commanded_manual_airspeed_setpoint;
+		altctrl_airspeed = constrain(_commanded_manual_airspeed_setpoint,
+					     _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()),
+					     _performance_model.getMaximumCalibratedAirspeed());
 	}
 
 	return altctrl_airspeed;
@@ -407,36 +357,16 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
 {
 	// --- airspeed *constraint adjustments ---
 
-	float load_factor_from_bank_angle = 1.0f;
-
-	if (PX4_ISFINITE(_att_sp.roll_body)) {
-		load_factor_from_bank_angle = 1.0f / cosf(_att_sp.roll_body);
-	}
-
-	float weight_ratio = 1.0f;
-
-	if (_param_weight_base.get() > FLT_EPSILON && _param_weight_gross.get() > FLT_EPSILON) {
-		weight_ratio = math::constrain(_param_weight_gross.get() / _param_weight_base.get(), MIN_WEIGHT_RATIO,
-					       MAX_WEIGHT_RATIO);
-	}
-
-	// Here we make sure that the set minimum airspeed is automatically adapted to the current load factor.
-	// The minimum airspeed is the controller limit (FW_AIRSPD_MIN, unless either in takeoff or landing) that should
-	// resemble the vehicles stall speed (CAS) with a 1g load plus some safety margin (as no controller tracks perfectly).
-	// Stall speed increases with the square root of the load factor: V_stall ~ sqrt(load_factor).
-	// The load_factor is composed of a term from the bank angle and a term from the weight ratio.
-	calibrated_min_airspeed *= sqrtf(load_factor_from_bank_angle * weight_ratio);
-
 	// Aditional option to increase the min airspeed setpoint based on wind estimate for more stability in higher winds.
 	if (!in_takeoff_situation && _airspeed_valid && _wind_valid && _param_fw_wind_arsp_sc.get() > FLT_EPSILON) {
 		calibrated_min_airspeed = math::min(calibrated_min_airspeed + _param_fw_wind_arsp_sc.get() *
-						    _wind_vel.length(), _param_fw_airspd_max.get());
+						    _wind_vel.length(), _performance_model.getMaximumCalibratedAirspeed());
 	}
 
 	// --- airspeed *setpoint adjustments ---
 
 	if (!PX4_ISFINITE(calibrated_airspeed_setpoint) || calibrated_airspeed_setpoint <= FLT_EPSILON) {
-		calibrated_airspeed_setpoint = _param_fw_airspd_trim.get();
+		calibrated_airspeed_setpoint = _performance_model.getCalibratedTrimAirspeed();
 	}
 
 	// Adapt cruise airspeed when otherwise the min groundspeed couldn't be maintained
@@ -455,7 +385,7 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
 	}
 
 	calibrated_airspeed_setpoint = constrain(calibrated_airspeed_setpoint, calibrated_min_airspeed,
-				       _param_fw_airspd_max.get());
+				       _performance_model.getMaximumCalibratedAirspeed());
 
 	if (!PX4_ISFINITE(_airspeed_slew_rate_controller.getState())) {
 		_airspeed_slew_rate_controller.setForcedValue(calibrated_airspeed_setpoint);
@@ -470,8 +400,8 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
 		_airspeed_slew_rate_controller.setForcedValue(calibrated_min_airspeed);
 	}
 
-	if (_airspeed_slew_rate_controller.getState() > _param_fw_airspd_max.get()) {
+	if (_airspeed_slew_rate_controller.getState() > _performance_model.getMaximumCalibratedAirspeed()) {
-		_airspeed_slew_rate_controller.setForcedValue(_param_fw_airspd_max.get());
+		_airspeed_slew_rate_controller.setForcedValue(_performance_model.getMaximumCalibratedAirspeed());
 	}
 
 	return _airspeed_slew_rate_controller.getState();
@@ -694,7 +624,7 @@ void
 FixedwingPositionControl::updateManualTakeoffStatus()
 {
 	if (!_completed_manual_takeoff) {
-		const bool at_controllable_airspeed = _airspeed > _param_fw_airspd_min.get()
+		const bool at_controllable_airspeed = _airspeed > _performance_model.getMinimumCalibratedAirspeed(getLoadFactor())
 						      || !_airspeed_valid;
 		const bool is_hovering = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
 					 && _control_mode.flag_armed;
@@ -966,7 +896,7 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i
 
 	tecs_update_pitch_throttle(control_interval,
 				   _current_altitude,
-				   _param_fw_airspd_trim.get(),
+				   _performance_model.getCalibratedTrimAirspeed(),
 				   radians(_param_fw_p_lim_min.get()),
 				   radians(_param_fw_p_lim_max.get()),
 				   _param_fw_thr_min.get(),
@@ -997,7 +927,7 @@ FixedwingPositionControl::control_auto_descend(const float control_interval)
 
 	tecs_update_pitch_throttle(control_interval,
 				   _current_altitude,
-				   _param_fw_airspd_trim.get(),
+				   _performance_model.getCalibratedTrimAirspeed(),
 				   radians(_param_fw_p_lim_min.get()),
 				   radians(_param_fw_p_lim_max.get()),
 				   _param_fw_thr_min.get(),
@@ -1113,13 +1043,13 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
 	}
 
 	float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed,
-				_param_fw_airspd_min.get(), ground_speed);
+				_performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed);
 
 	Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
 	Vector2f curr_wp_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);
 
 	_npfg.setAirspeedNom(target_airspeed * _eas2tas);
-	_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+	_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
 
 	if (_control_mode.flag_control_offboard_enabled && PX4_ISFINITE(pos_sp_curr.vx) && PX4_ISFINITE(pos_sp_curr.vy)) {
 		// Navigate directly on position setpoint and path tangent
@@ -1180,11 +1110,11 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
 	_target_bearing = wrap_pi(atan2f(target_velocity(1), target_velocity(0)));
 
 	float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed,
-				_param_fw_airspd_min.get(), ground_speed);
+				_performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed);
 
 	Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
 	_npfg.setAirspeedNom(target_airspeed * _eas2tas);
-	_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+	_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
 	navigateBearing(curr_pos_local, _target_bearing, ground_speed, _wind_vel);
 	_att_sp.roll_body = _npfg.getRollSetpoint();
 	target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
@@ -1265,17 +1195,19 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 		// landing airspeed and potentially tighter altitude control) already such that we don't
 		// have to do this switch (which can cause significant altitude errors) close to the ground.
 		_tecs.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
-		airspeed_sp = (_param_fw_lnd_airspd.get() > FLT_EPSILON) ? _param_fw_lnd_airspd.get() : _param_fw_airspd_min.get();
+		airspeed_sp = (_param_fw_lnd_airspd.get() > FLT_EPSILON) ? _param_fw_lnd_airspd.get() :
+			      _performance_model.getMinimumCalibratedAirspeed(getLoadFactor());
 		_flaps_setpoint = _param_fw_flaps_lnd_scl.get();
 		_spoilers_setpoint = _param_fw_spoilers_lnd.get();
 		_new_landing_gear_position = landing_gear_s::GEAR_DOWN;
 	}
 
-	float target_airspeed = adapt_airspeed_setpoint(control_interval, airspeed_sp, _param_fw_airspd_min.get(),
+	float target_airspeed = adapt_airspeed_setpoint(control_interval, airspeed_sp,
+				_performance_model.getMinimumCalibratedAirspeed(getLoadFactor()),
 				ground_speed);
 
 	_npfg.setAirspeedNom(target_airspeed * _eas2tas);
-	_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+	_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
 	navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise,
 		       ground_speed,
 		       _wind_vel);
@@ -1317,7 +1249,7 @@ FixedwingPositionControl::controlAutoFigureEight(const float control_interval, c
 {
 	// airspeed settings
 	float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed,
-				_param_fw_airspd_min.get(), ground_speed);
+				_performance_model.getMinimumCalibratedAirspeed(), ground_speed);
 
 	// Lateral Control
 
@@ -1403,11 +1335,11 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 	Vector2f local_2D_position{_local_pos.x, _local_pos.y};
 
 	const float takeoff_airspeed = (_param_fw_tko_airspd.get() > FLT_EPSILON) ? _param_fw_tko_airspd.get() :
-				       _param_fw_airspd_min.get();
+				       _performance_model.getMinimumCalibratedAirspeed(getLoadFactor());
 
-	float adjusted_min_airspeed = _param_fw_airspd_min.get();
+	float adjusted_min_airspeed = _performance_model.getMinimumCalibratedAirspeed(getLoadFactor());
 
-	if (takeoff_airspeed < _param_fw_airspd_min.get()) {
+	if (takeoff_airspeed < adjusted_min_airspeed) {
 		// adjust underspeed detection bounds for takeoff airspeed
 		_tecs.set_equivalent_airspeed_min(takeoff_airspeed);
 		adjusted_min_airspeed = takeoff_airspeed;
@@ -1464,7 +1396,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 					true);
 
 		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
-		_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+		_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
 		navigateLine(start_pos_local, takeoff_bearing, local_2D_position, ground_speed, _wind_vel);
 
 		_att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint());
@@ -1498,9 +1430,9 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 					   _param_fw_thr_min.get(),
 					   _param_fw_thr_max.get(),
 					   _param_sinkrate_target.get(),
-					   _param_fw_t_clmb_max.get());
+					   _performance_model.getMaximumClimbRate(_air_density));
 
-		_tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); // reset after TECS calculation
+		_tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation
 
 		_att_sp.pitch_body = _runway_takeoff.getPitch(get_tecs_pitch());
 		_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(_param_fw_thr_idle.get(), get_tecs_thrust());
@@ -1563,7 +1495,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 						true);
 
 			_npfg.setAirspeedNom(target_airspeed * _eas2tas);
-			_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+			_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
 			navigateLine(launch_local_position, takeoff_bearing, local_2D_position, ground_speed, _wind_vel);
 			_att_sp.roll_body = _npfg.getRollSetpoint();
 			target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
@@ -1580,7 +1512,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 						   _param_fw_thr_min.get(),
 						   max_takeoff_throttle,
 						   _param_sinkrate_target.get(),
-						   _param_fw_t_clmb_max.get());
+						   _performance_model.getMaximumClimbRate(_air_density));
 
 			if (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) {
 				// explicitly set idle throttle until motors are enabled
@@ -1622,10 +1554,10 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
 {
 	// first handle non-position things like airspeed and tecs settings
 	const float airspeed_land = (_param_fw_lnd_airspd.get() > FLT_EPSILON) ? _param_fw_lnd_airspd.get() :
-				    _param_fw_airspd_min.get();
+				    _performance_model.getMinimumCalibratedAirspeed(getLoadFactor());
-	float adjusted_min_airspeed = _param_fw_airspd_min.get();
+	float adjusted_min_airspeed = _performance_model.getMinimumCalibratedAirspeed(getLoadFactor());
 
-	if (airspeed_land < _param_fw_airspd_min.get()) {
+	if (airspeed_land < adjusted_min_airspeed) {
 		// adjust underspeed detection bounds for landing airspeed
 		_tecs.set_equivalent_airspeed_min(airspeed_land);
 		adjusted_min_airspeed = airspeed_land;
@@ -1711,7 +1643,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
 		const Vector2f local_approach_entrance = local_land_point - landing_approach_vector;
 
 		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
-		_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+		_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
 		navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel);
 		target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 		_att_sp.roll_body = _npfg.getRollSetpoint();
@@ -1790,7 +1722,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
 		const Vector2f local_approach_entrance = local_land_point - landing_approach_vector;
 
 		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
-		_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+		_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
 		navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel);
 		target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 		_att_sp.roll_body = _npfg.getRollSetpoint();
@@ -1826,7 +1758,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
 		_att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_idle.get() : get_tecs_thrust();
 	}
 
-	_tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); // reset after TECS calculation
+	_tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation
 
 	_att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt);
 
@@ -1849,10 +1781,10 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
 {
 	// first handle non-position things like airspeed and tecs settings
 	const float airspeed_land = (_param_fw_lnd_airspd.get() > FLT_EPSILON) ? _param_fw_lnd_airspd.get() :
-				    _param_fw_airspd_min.get();
+				    _performance_model.getMinimumCalibratedAirspeed(getLoadFactor());
-	float adjusted_min_airspeed = _param_fw_airspd_min.get();
+	float adjusted_min_airspeed = _performance_model.getMinimumCalibratedAirspeed(getLoadFactor());
 
-	if (airspeed_land < _param_fw_airspd_min.get()) {
+	if (airspeed_land < adjusted_min_airspeed) {
 		// adjust underspeed detection bounds for landing airspeed
 		_tecs.set_equivalent_airspeed_min(airspeed_land);
 		adjusted_min_airspeed = airspeed_land;
@@ -1913,7 +1845,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
 
 		_npfg.setPeriod(ground_roll_npfg_period);
 		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
-		_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+		_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
 
 		navigateLoiter(local_landing_orbit_center, local_position, loiter_radius,
 			       pos_sp_curr.loiter_direction_counter_clockwise,
@@ -1990,7 +1922,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
 
 		/* lateral guidance */
 		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
-		_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+		_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
 
 		navigateLoiter(local_landing_orbit_center, local_position, loiter_radius,
 			       pos_sp_curr.loiter_direction_counter_clockwise,
@@ -2031,7 +1963,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
 		_att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_idle.get() : get_tecs_thrust();
 	}
 
-	_tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); // reset after TECS calculation
+	_tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation
 
 	_att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt);
 
@@ -2053,7 +1985,7 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval,
 	updateManualTakeoffStatus();
 
 	const float calibrated_airspeed_sp = adapt_airspeed_setpoint(control_interval, get_manual_airspeed_setpoint(),
-					     _param_fw_airspd_min.get(), ground_speed, !_completed_manual_takeoff);
+					     _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed, !_completed_manual_takeoff);
 	const float height_rate_sp = getManualHeightRateSetpoint();
 
 	// TECS may try to pitch down to gain airspeed if we underspeed, constrain the pitch when underspeeding if we are
@@ -2094,7 +2026,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
 	updateManualTakeoffStatus();
 
 	float calibrated_airspeed_sp = adapt_airspeed_setpoint(control_interval, get_manual_airspeed_setpoint(),
-				       _param_fw_airspd_min.get(), ground_speed, !_completed_manual_takeoff);
+				       _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed, !_completed_manual_takeoff);
 	const float height_rate_sp = getManualHeightRateSetpoint();
 
 	// TECS may try to pitch down to gain airspeed if we underspeed, constrain the pitch when underspeeding if we are
@@ -2143,7 +2075,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
 			Vector2f curr_wp_local = _global_local_proj_ref.project(_hdg_hold_position.lat, _hdg_hold_position.lon);
 
 			_npfg.setAirspeedNom(calibrated_airspeed_sp * _eas2tas);
-			_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+			_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
 			navigateLine(curr_wp_local, _hdg_hold_yaw, curr_pos_local, ground_speed, _wind_vel);
 			_att_sp.roll_body = _npfg.getRollSetpoint();
 			calibrated_airspeed_sp = _npfg.getAirspeedRef() / _eas2tas;
@@ -2378,6 +2310,8 @@ FixedwingPositionControl::Run()
 
 		if (_vehicle_air_data_sub.update(&air_data)) {
 			_air_density = PX4_ISFINITE(air_data.rho) ? air_data.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_land_detected_sub.updated()) {
@@ -2572,48 +2506,6 @@ FixedwingPositionControl::reset_landing_state()
 	}
 }
 
-float FixedwingPositionControl::calculateTrimThrottle(float throttle_min,
-		float throttle_max, float airspeed_sp)
-{
-	float throttle_trim =
-		_param_fw_thr_trim.get(); // throttle required for level flight at trim airspeed, at sea level (standard atmosphere)
-
-	// Drag modelling (parasite drag): calculate mapping airspeed-->throttle, assuming a linear relation with different gradients
-	// above and below trim. This is tunable thorugh FW_THR_ASPD_MIN and FW_THR_ASPD_MAX.
-	const float slope_below_trim = (_param_fw_thr_trim.get() - _param_fw_thr_aspd_min.get()) /
-				       (_param_fw_airspd_trim.get() - _param_fw_airspd_min.get());
-	const float slope_above_trim = (_param_fw_thr_aspd_max.get() - _param_fw_thr_trim.get()) /
-				       (_param_fw_airspd_max.get() - _param_fw_airspd_trim.get());
-
-	if (PX4_ISFINITE(airspeed_sp) && PX4_ISFINITE(slope_below_trim) && _param_fw_thr_aspd_min.get() > FLT_EPSILON
-	    && airspeed_sp < _param_fw_airspd_trim.get()) {
-		throttle_trim = _param_fw_thr_trim.get() - slope_below_trim * (_param_fw_airspd_trim.get() - airspeed_sp);
-
-	} else if (PX4_ISFINITE(airspeed_sp) && PX4_ISFINITE(slope_above_trim) && _param_fw_thr_aspd_max.get() > FLT_EPSILON
-		   && airspeed_sp > _param_fw_airspd_trim.get()) {
-		throttle_trim = _param_fw_thr_trim.get() + slope_above_trim * (airspeed_sp - _param_fw_airspd_trim.get());
-	}
-
-	float weight_ratio = 1.0f;
-
-	if (_param_weight_base.get() > FLT_EPSILON && _param_weight_gross.get() > FLT_EPSILON) {
-		weight_ratio = math::constrain(_param_weight_gross.get() / _param_weight_base.get(), MIN_WEIGHT_RATIO,
-					       MAX_WEIGHT_RATIO);
-	}
-
-	float air_density_throttle_scale = 1.0f;
-
-	if (PX4_ISFINITE(_air_density)) {
-		// scale throttle as a function of sqrt(rho0/rho)
-		const float eas2tas = sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / _air_density);
-		const float eas2tas_at_5000m_amsl = sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / AIR_DENSITY_STANDARD_ATMOS_5000_AMSL);
-		air_density_throttle_scale = constrain(eas2tas, 1.f, eas2tas_at_5000m_amsl);
-	}
-
-	// compensate trim throttle for both weight and air density
-	return math::constrain(throttle_trim * sqrtf(weight_ratio) * air_density_throttle_scale, throttle_min, throttle_max);
-}
-
 void
 FixedwingPositionControl::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,
@@ -2642,8 +2534,9 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 				_airspeed_after_transition = _airspeed;
 			}
 
-			_airspeed_after_transition = constrain(_airspeed_after_transition, _param_fw_airspd_min.get(),
+			_airspeed_after_transition = constrain(_airspeed_after_transition,
-							       _param_fw_airspd_max.get());
+							       _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()),
+							       _performance_model.getMaximumCalibratedAirspeed());
 
 		} else if (_was_in_transition) {
 			// after transition we ramp up desired airspeed from the speed we had coming out of the transition
@@ -2683,8 +2576,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 	}
 
 	/* update TECS vehicle state estimates */
-	const float throttle_trim_adjusted = calculateTrimThrottle(throttle_min,
+	const float throttle_trim_compensated = _performance_model.getTrimThrottle(throttle_min,
-					     throttle_max, airspeed_sp);
+						throttle_max, airspeed_sp, _air_density);
 
 	// 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.
@@ -2698,8 +2591,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 		     _eas2tas,
 		     throttle_min,
 		     throttle_max,
-		     _param_fw_thr_trim.get(),
+		     throttle_trim_compensated,
-		     throttle_trim_adjusted,
 		     pitch_min_rad - radians(_param_fw_psp_off.get()),
 		     pitch_max_rad - radians(_param_fw_psp_off.get()),
 		     desired_max_climbrate,
@@ -2708,7 +2600,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 		     -_local_pos.vz,
 		     hgt_rate_sp);
 
-	tecs_status_publish(alt_sp, airspeed_sp, airspeed_rate_estimate, throttle_trim_adjusted);
+	tecs_status_publish(alt_sp, airspeed_sp, airspeed_rate_estimate, throttle_trim_compensated);
 }
 
 float
@@ -3144,6 +3036,17 @@ fw_pos_control is the fixed-wing position controller.
 
 	return 0;
 }
+float FixedwingPositionControl::getLoadFactor()
+{
+	float load_factor_from_bank_angle = 1.0f;
+
+	if (PX4_ISFINITE(_att_sp.roll_body)) {
+		load_factor_from_bank_angle = 1.0f / math::max(cosf(_att_sp.roll_body), FLT_EPSILON);
+	}
+
+	return load_factor_from_bank_angle;
+
+}
 
 extern "C" __EXPORT int fw_pos_control_main(int argc, char *argv[])
 {

src/modules/fw_pos_control/FixedwingPositionControl.hpp

@@ -49,11 +49,13 @@
 
 #include "launchdetection/LaunchDetector.h"
 #include "runway_takeoff/RunwayTakeoff.h"
+#include <lib/fw_performance_model/PerformanceModel.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/mathlib/mathlib.h>
@@ -146,15 +148,6 @@ static constexpr float MIN_AUTO_TIMESTEP = 0.01f;
 // [s] maximum time step between auto control updates
 static constexpr float MAX_AUTO_TIMESTEP = 0.05f;
 
-// [.] minimum ratio between the actual vehicle weight and the vehicle nominal weight (weight at which the performance limits are derived)
-static constexpr float MIN_WEIGHT_RATIO = 0.5f;
-
-// [.] maximum ratio between the actual vehicle weight and the vehicle nominal weight (weight at which the performance limits are derived)
-static constexpr float MAX_WEIGHT_RATIO = 2.0f;
-
-// air density of standard athmosphere at 5000m above mean sea level [kg/m^3]
-static constexpr float AIR_DENSITY_STANDARD_ATMOS_5000_AMSL = 0.7363f;
-
 // [rad] minimum pitch while airspeed has not yet reached a controllable value in manual position controlled takeoff modes
 static constexpr float MIN_PITCH_DURING_MANUAL_TAKEOFF = 0.0f;
 
@@ -375,7 +368,7 @@ private:
 	float _airspeed{0.0f};
 	float _eas2tas{1.0f};
 	bool _airspeed_valid{false};
-	float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C};
+	float _air_density{atmosphere::kAirDensitySeaLevelStandardAtmos};
 
 	// [us] last time airspeed was received. used to detect timeouts.
 	hrt_abstime _time_airspeed_last_valid{0};
@@ -423,6 +416,8 @@ private:
 	// nonlinear path following guidance - lateral-directional position control
 	NPFG _npfg;
 
+	PerformanceModel _performance_model;
+
 	// LANDING GEAR
 	int8_t _new_landing_gear_position{landing_gear_s::GEAR_KEEP};
 
@@ -454,7 +449,7 @@ private:
 #endif // CONFIG_FIGURE_OF_EIGHT
 
 	// Update our local parameter cache.
-	int parameters_update();
+	void parameters_update();
 
 	// Update subscriptions
 	void airspeed_poll();
@@ -471,6 +466,7 @@ private:
 	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 Sets the landing abort status and publishes landing status.
@@ -713,17 +709,6 @@ private:
 	 */
 	void set_control_mode_current(const hrt_abstime &now);
 
-	/**
-	 * @brief Estimate trim throttle for air density, vehicle weight and current airspeed
-	 *
-	 * @param throttle_min Minimum allowed trim throttle.
-	 * @param throttle_max Maximum allowed trim throttle.
-	 * @param airspeed_sp Current airspeed setpoint (CAS) [m/s]
-	 * @return Estimated trim throttle
-	 */
-	float calculateTrimThrottle(float throttle_min, float throttle_max,
-				    float airspeed_sp);
-
 	void publishOrbitStatus(const position_setpoint_s pos_sp);
 
 	SlewRate<float> _airspeed_slew_rate_controller;
@@ -909,12 +894,6 @@ private:
 			     const matrix::Vector2f &wind_vel);
 
 	DEFINE_PARAMETERS(
-
-		(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_AIRSPD_STALL>) _param_fw_airspd_stall,
-
 		(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,
@@ -944,7 +923,6 @@ private:
 		(ParamFloat<px4::params::FW_P_LIM_MAX>) _param_fw_p_lim_max,
 		(ParamFloat<px4::params::FW_P_LIM_MIN>) _param_fw_p_lim_min,
 
-		(ParamFloat<px4::params::FW_T_CLMB_MAX>) _param_fw_t_clmb_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_I_GAIN_THR>) _param_fw_t_I_gain_thr,
@@ -952,7 +930,6 @@ private:
 		(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_SINK_MIN>) _param_fw_t_sink_min,
 		(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_DAMP>) _param_fw_t_thr_damp,
@@ -965,10 +942,6 @@ private:
 		(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_ASPD_MIN>) _param_fw_thr_aspd_min,
-		(ParamFloat<px4::params::FW_THR_ASPD_MAX>) _param_fw_thr_aspd_max,
-
-		(ParamFloat<px4::params::FW_THR_TRIM>) _param_fw_thr_trim,
 		(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,
@@ -995,9 +968,6 @@ private:
 
 		(ParamFloat<px4::params::NAV_FW_ALT_RAD>) _param_nav_fw_alt_rad,
 
-		(ParamFloat<px4::params::WEIGHT_BASE>) _param_weight_base,
-		(ParamFloat<px4::params::WEIGHT_GROSS>) _param_weight_gross,
-
 		(ParamFloat<px4::params::FW_WING_SPAN>) _param_fw_wing_span,
 		(ParamFloat<px4::params::FW_WING_HEIGHT>) _param_fw_wing_height,
 

src/modules/fw_pos_control/fw_path_navigation_params.c

@@ -177,19 +177,6 @@ PARAM_DEFINE_FLOAT(NPFG_SW_DST_MLT, 0.32f);
  */
 PARAM_DEFINE_FLOAT(NPFG_PERIOD_SF, 1.5f);
 
-/**
- * Trim throttle
- *
- * This is the throttle setting required to achieve FW_AIRSPD_TRIM during level flight.
- *
- * @unit norm
- * @min 0.0
- * @max 1.0
- * @decimal 2
- * @increment 0.01
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_THR_TRIM, 0.6f);
 
 /**
  * Throttle max slew rate
@@ -447,46 +434,11 @@ PARAM_DEFINE_FLOAT(FW_LND_AIRSPD, -1.f);
  */
 PARAM_DEFINE_FLOAT(FW_LND_THRTC_SC, 1.0f);
 
-
-
 /*
  * TECS parameters
  *
  */
 
-/**
- * Maximum climb rate
- *
- * This is the maximum climb rate that the aircraft can achieve with
- * the throttle set to THR_MAX and the airspeed set to the
- * trim value. For electric aircraft make sure this number can be
- * achieved towards the end of flight when the battery voltage has reduced.
- *
- * @unit m/s
- * @min 1.0
- * @max 15.0
- * @decimal 1
- * @increment 0.5
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
-
-/**
- * Minimum descent rate
- *
- * This is the sink rate of the aircraft with the throttle
- * set to THR_MIN and flown at the same airspeed as used
- * to measure FW_T_CLMB_MAX.
- *
- * @unit m/s
- * @min 1.0
- * @max 5.0
- * @decimal 1
- * @increment 0.5
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
-
 /**
  * Maximum descent rate
  *
@@ -818,32 +770,6 @@ PARAM_DEFINE_INT32(FW_GPSF_LT, 30);
  */
 PARAM_DEFINE_FLOAT(FW_GPSF_R, 15.0f);
 
-/**
- * Vehicle base weight.
- *
- * This is the weight of the vehicle at which it's performance limits were derived. A zero or negative value
- * disables trim throttle and minimum airspeed compensation based on weight.
- *
- * @unit kg
- * @decimal 1
- * @increment 0.5
- * @group Mission
- */
-PARAM_DEFINE_FLOAT(WEIGHT_BASE, -1.0f);
-
-/**
- * Vehicle gross weight.
- *
- * 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.
- *
- * @unit kg
- * @decimal 1
- * @increment 0.1
- * @group Mission
- */
-PARAM_DEFINE_FLOAT(WEIGHT_GROSS, -1.0f);
 
 /**
  * The aircraft's wing span (length from tip to tip).
@@ -1054,33 +980,3 @@ PARAM_DEFINE_FLOAT(FW_SPOILERS_LND, 0.f);
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_SPOILERS_DESC, 0.f);
-
-/**
- * Throttle at min airspeed
- *
- * Required throttle for level flight at minimum airspeed FW_AIRSPD_MIN (sea level, standard atmosphere)
- *
- * Set to 0 to disable mapping of airspeed to trim throttle below FW_AIRSPD_TRIM.
- *
- * @min 0
- * @max 1
- * @decimal 2
- * @increment 0.01
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_THR_ASPD_MIN, 0.f);
-
-/**
- * Throttle at max airspeed
- *
- * Required throttle for level flight at maximum airspeed FW_AIRSPD_MAX (sea level, standard atmosphere)
- *
- * Set to 0 to disable mapping of airspeed to trim throttle.
- *
- * @min 0
- * @max 1
- * @decimal 2
- * @increment 0.01
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_THR_ASPD_MAX, 0.f);

src/modules/fw_rate_control/fw_rate_control_params.c

@@ -40,37 +40,6 @@
  * @author Thomas Gubler <thomas@px4.io>
  */
 
-/**
- * Minimum Airspeed (CAS)
- *
- * The minimal airspeed (calibrated airspeed) the user is able to command.
- * Further, if the airspeed falls below this value, the TECS controller will try to
- * increase airspeed more aggressively.
- * Should be set (with some margin) above the vehicle stall speed.
- * This value corresponds to the desired minimum speed with the default load factor (level flight, default weight),
- * and is automatically adapated to the current load factor (calculated from roll setpoint and WEIGHT_GROSS/WEIGHT_BASE).
- *
- * @unit m/s
- * @min 0.5
- * @decimal 1
- * @increment 0.5
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f);
-
-/**
- * Maximum Airspeed (CAS)
- *
- * The maximal airspeed (calibrated airspeed) the user is able to command.
- *
- * @unit m/s
- * @min 0.5
- * @decimal 1
- * @increment 0.5
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f);
-
 /**
  * Airspeed mode
  *
@@ -83,35 +52,6 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f);
  */
 PARAM_DEFINE_INT32(FW_ARSP_MODE, 0);
 
-/**
- * Trim (Cruise) Airspeed
- *
- * The trim CAS (calibrated airspeed) of the vehicle. If an airspeed controller is active,
- * this is the default airspeed setpoint that the controller will try to achieve.
- *
- * @unit m/s
- * @min 0.5
- * @decimal 1
- * @increment 0.5
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f);
-
-/**
- * Stall Airspeed (CAS)
- *
- * The stall airspeed (calibrated airspeed) of the vehicle.
- * It is used for airspeed sensor failure detection and for the control
- * surface scaling airspeed limits.
- *
- * @unit m/s
- * @min 0.5
- * @decimal 1
- * @increment 0.5
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_AIRSPD_STALL, 7.0f);
-
 /**
  * Pitch rate proportional gain.
  *

src/modules/sensors/vehicle_air_data/VehicleAirData.cpp

@@ -269,6 +269,7 @@ void VehicleAirData::Run()
 						out.baro_temp_celcius = temperature;
 						out.baro_pressure_pa = pressure_pa;
 						out.rho = air_density;
+						out.eas2tas = sqrtf(kAirDensitySeaLevelStandardAtmos / math::max(air_density, FLT_EPSILON));
 						out.calibration_count = _calibration[instance].calibration_count();
 						out.timestamp = hrt_absolute_time();
 

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -35,7 +35,7 @@
 
 #include <uORB/Subscription.hpp>
 
-#include <lib/geo/geo.h>
+#include <lib/atmosphere/atmosphere.h>
 #include <lib/mathlib/mathlib.h>
 
 #include <px4_platform_common/getopt.h>
@@ -388,7 +388,7 @@ void GZBridge::airspeedCallback(const gz::msgs::AirSpeedSensor &air_speed)
 	report.timestamp_sample = time_us;
 	report.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION
 	report.differential_pressure_pa = static_cast<float>(air_speed_value); // hPa to Pa;
-	report.temperature = static_cast<float>(air_speed.temperature()) + CONSTANTS_ABSOLUTE_NULL_CELSIUS; // K to C
+	report.temperature = static_cast<float>(air_speed.temperature()) + atmosphere::kAbsoluteNullCelsius; // K to C
 	report.timestamp = hrt_absolute_time();;
 	_differential_pressure_pub.publish(report);
 

src/modules/simulation/simulator_sih/sih.hpp

@@ -61,7 +61,7 @@
 
 #include <matrix/matrix/math.hpp>   // matrix, vectors, dcm, quaterions
 #include <conversion/rotation.h>    // math::radians,
-#include <lib/geo/geo.h>        // to get the physical constants
+#include <lib/atmosphere/atmosphere.h>        // to get the physical constants
 #include <drivers/drv_hrt.h>        // to get the real time
 #include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
 #include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
@@ -134,7 +134,7 @@ private:
 	// hard constants
 	static constexpr uint16_t NB_MOTORS = 6;
 	static constexpr float T1_C = 15.0f;                        // ground temperature in Celsius
-	static constexpr float T1_K = T1_C - CONSTANTS_ABSOLUTE_NULL_CELSIUS;   // ground temperature in Kelvin
+	static constexpr float T1_K = T1_C - atmosphere::kAbsoluteNullCelsius;   // ground temperature in Kelvin
 	static constexpr float TEMP_GRADIENT = -6.5f / 1000.0f;    // temperature gradient in degrees per metre
 	// Aerodynamic coefficients
 	static constexpr float RHO = 1.225f; 		// air density at sea level [kg/m^3]

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -50,7 +50,7 @@
 #pragma once
 
 #include <drivers/drv_hrt.h>
-#include <lib/geo/geo.h>
+#include <lib/atmosphere/atmosphere.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/perf/perf_counter.h>
 #include <matrix/math.hpp>
@@ -209,7 +209,7 @@ private:
 	vtol_vehicle_status_s 			_vtol_vehicle_status{};
 	float _home_position_z{NAN};
 
-	float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C};	// [kg/m^3]
+	float _air_density{atmosphere::kAirDensitySeaLevelStandardAtmos};	// [kg/m^3]
 
 	hrt_abstime _last_run_timestamp{0};
 

src/modules/vtol_att_control/vtol_type.cpp

@@ -45,6 +45,7 @@
 #include <float.h>
 #include <px4_platform_common/defines.h>
 #include <matrix/math.hpp>
+#include <lib/atmosphere/atmosphere.h>
 
 using namespace matrix;
 
@@ -565,7 +566,7 @@ float VtolType::getFrontTransitionTimeFactor() const
 	const float rho = math::constrain(_attc->getAirDensity(), 0.7f, 1.5f);
 
 	if (PX4_ISFINITE(rho)) {
-		float rho0_over_rho = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / rho;
+		float rho0_over_rho = atmosphere::kAirDensitySeaLevelStandardAtmos / rho;
 		return sqrtf(rho0_over_rho) * rho0_over_rho;
 	}
 

validation/module_schema.yaml

@@ -147,7 +147,7 @@ parameters:
                                 'bit/s', 'B/s',
                                 'deg', 'deg*1e7', 'deg/s',
                                 'celcius', 'gauss', 'gauss/s', 'mgauss', 'mgauss^2',
-                                'hPa', 'kg', 'kg/m^2', 'kg m^2',
+                                'hPa', 'kg', 'kg/m^2', 'kg m^2', 'kg/m^3',
                                 'mm', 'm', 'm/s', 'm^2', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', 'm/s/rad',
                                 'Ohm', 'V', 'A',
                                 'us', 'ms', 's',