mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-25 08:13:33 +08:00
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>
This commit is contained in:
Roman Bapst
co-authored by
Thomas Stastny
Thomas Stastny
parent
70cd224236
commit
dd2322d622
@@ -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)
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
|
||||
}
|
||||
|
||||
@@ -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.
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
@@ -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
|
||||
)
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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_
|
||||
@@ -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);
|
||||
@@ -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)
|
||||
|
||||
|
||||
|
||||
@@ -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',
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
@@ -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_adjusted; ///< Trim throttle adjusted for airspeed, load factor and air density
|
||||
float throttle_trim; ///< Normalized throttle required to fly level at calibrated airspeed setpoint [0,1]
|
||||
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,
|
||||
|
||||
Reference in New Issue
Block a user