/**************************************************************************** * * 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 #include #include "PerformanceModel.hpp" #include 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 * - FW_AIRSPD_MAX: {1:.1} * - FW_AIRSPD_MIN: {2:.1} */ events::send(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 * - FW_AIRSPD_MAX: {1:.1} * - FW_AIRSPD_MIN: {2:.1} */ events::send(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 * - FW_AIRSPD_MAX: {1:.1} * - FW_AIRSPD_MIN: {2:.1} * - FW_AIRSPD_TRIM: {3:.1} */ events::send(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 * - FW_AIRSPD_MIN: {1:.1} * - FW_AIRSPD_STALL: {2:.1} */ events::send(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); }