Added PerformanceModel for fixed wing

msg/TecsStatus.msg

@@ -25,5 +25,6 @@ float32 pitch_integ			# Pitch integrator value [rad]
 float32 throttle_sp			# Current throttle setpoint [-]
 float32 pitch_sp_rad			# Current pitch setpoint [rad]
 float32 throttle_trim			# estimated throttle value [0,1] required to fly level at equivalent_airspeed_sp in the current atmospheric conditions
+float32 throttle_trim_calibrated # estimated trim throttle value [0,1] required to fly level at equivalent_airspeed_sp at sea level in standard atmosphere
 
 float32 underspeed_ratio		# 0: no underspeed, 1: maximal underspeed. Controller takes measures to avoid stall proportional to ratio if >0.

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 @@ class BatteryInfo :
 			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 @@ class RawAirData :
 			// 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.static_air_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 - 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 @@ class StaticTemperature :
 
 		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
+)