diff --git a/src/lib/atmosphere/atmosphere.cpp b/src/lib/atmosphere/atmosphere.cpp index 26040fca514d..5500568a2b39 100644 --- a/src/lib/atmosphere/atmosphere.cpp +++ b/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 kPa 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 - kAbsoluteNullCelcius))); } 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; } diff --git a/src/lib/atmosphere/atmosphere.h b/src/lib/atmosphere/atmosphere.h index 849a4303470b..72e85b8f3508 100644 --- a/src/lib/atmosphere/atmosphere.h +++ b/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 kAbsoluteNullCelcius = -273.15f; // °C +static constexpr float kTempRefKelvin = 15.f - kAbsoluteNullCelcius; // 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. diff --git a/src/lib/fw_performance_model/PerformanceModel.cpp b/src/lib/fw_performance_model/PerformanceModel.cpp index c37ca79c6ed1..f43da88374a3 100644 --- a/src/lib/fw_performance_model/PerformanceModel.cpp +++ b/src/lib/fw_performance_model/PerformanceModel.cpp @@ -39,16 +39,11 @@ #include #include -#include "PerformanceModel.h" +#include "PerformanceModel.hpp" #include using namespace atmosphere; -// air density of standard athmosphere at 5000m above mean sea level [kg/m^3] -static constexpr float kAirDensityStandardAtmos5000Amsl = 0.7363f; - -// air density of standard athmosphere at 1000m above mean sea level [kg/m^3] -static constexpr float kAirDensityStandardAtmos1000Amsl = 1.112f; // [.] 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; @@ -56,8 +51,8 @@ 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; -// climbrate defining the service ceiling, used to compensate max climbrate based on air density -static constexpr float kClimbrateMin = 0.5f; // [m/s] +// [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) { @@ -76,18 +71,20 @@ float PerformanceModel::getWeightRatio() const } 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 > 0.0f) { - const float pressure = getPressureFromAltitude(service_ceiling); - const float density = getDensityFromPressureAndTemp(pressure, getStandardTemperatureAtAltitude(service_ceiling)); - const float density_gradient = math::max((_param_fw_t_clmb_max.get() - kClimbrateMin) / - (CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C - - density), 0.0f); - const float delta_rho = air_density - CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; - climbrate_max = math::constrain(_param_fw_t_clmb_max.get() + density_gradient * delta_rho, kClimbrateMin, + 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()); } @@ -107,14 +104,13 @@ float PerformanceModel::getWeightThrottleScale() const float PerformanceModel::getAirDensityThrottleScale(float air_density) const { + air_density = sanitiseAirDensity(air_density); 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 / kAirDensityStandardAtmos5000Amsl); - air_density_throttle_scale = math::constrain(eas2tas, 1.f, eas2tas_at_5000m_amsl); - } + // 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; } @@ -143,7 +139,8 @@ float PerformanceModel::getTrimThrottleForAirspeed(float airspeed_sp) const } float PerformanceModel::getMinimumSinkRate(float air_density) const { - return _param_fw_t_sink_min.get() * sqrtf(getWeightRatio() * CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / air_density); + air_density = sanitiseAirDensity(air_density); + return _param_fw_t_sink_min.get() * sqrtf(getWeightRatio() * kAirDensitySeaLevelStandardAtmos / air_density); } float PerformanceModel::getCalibratedTrimAirspeed() const { @@ -152,11 +149,14 @@ float PerformanceModel::getCalibratedTrimAirspeed() const } 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); } @@ -227,3 +227,12 @@ void PerformanceModel:: updateParameters() { updateParams(); } + +float PerformanceModel::sanitiseAirDensity(float air_density) +{ + if (!PX4_ISFINITE(air_density)) { + air_density = kAirDensitySeaLevelStandardAtmos; + } + + return math::max(air_density, kAirDensityStandardAtmos11000Amsl); +} diff --git a/src/lib/fw_performance_model/PerformanceModel.h b/src/lib/fw_performance_model/PerformanceModel.hpp similarity index 91% rename from src/lib/fw_performance_model/PerformanceModel.h rename to src/lib/fw_performance_model/PerformanceModel.hpp index 12ecaadce2fb..afd85e37589c 100644 --- a/src/lib/fw_performance_model/PerformanceModel.h +++ b/src/lib/fw_performance_model/PerformanceModel.hpp @@ -51,15 +51,15 @@ class PerformanceModel : public ModuleParams void updateParameters(); /** - * Get the maximum climb rate expected for current air density and weight. + * 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 expected for current air density and weight. - * @param air_density + * 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; @@ -71,12 +71,12 @@ class PerformanceModel : public ModuleParams float getWeightRatio() const; /** - * Get the trim throttle for the current airspeed setpoint as well as air densty and weight. - * @param throttle_min minimum throttle - * @param throttle_max maximum throttle + * 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 - * @return trim throttle + * @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; @@ -149,6 +149,8 @@ class PerformanceModel : public ModuleParams * @return throttle scale factor for weight */ float getWeightThrottleScale() const; + + static float sanitiseAirDensity(float air_density); }; #endif //PX4_SRC_MODULES_FW_POS_CONTROL_PERFORMANCEMODEL_H_ diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 8d38bd263253..c1fa7b1e6a6f 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -54,19 +54,7 @@ #include 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)