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 2023-11-21 17:13:50 +01:00 committed by GitHub
parent 70cd224236
commit dd2322d622
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
40 changed files with 800 additions and 474 deletions

View File

@ -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.

View File

@ -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) {

View File

@ -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) {

View File

@ -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);
}

View File

@ -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;

View File

@ -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

View File

@ -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;

View File

@ -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();

View File

@ -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;

View File

@ -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.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);

View File

@ -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

View File

@ -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)

View File

@ -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);
}

View File

@ -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;
}

View File

@ -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.

View File

@ -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;

View File

@ -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
)

View File

@ -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);
}

View File

@ -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_

View File

@ -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);

View File

@ -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)

View File

@ -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',

View File

@ -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;

View File

@ -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,

View File

@ -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)

View File

@ -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;

View File

@ -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;

View File

@ -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

View File

@ -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

View File

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

View File

@ -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_min(_param_fw_airspd_min.get());
_tecs.set_equivalent_airspeed_trim(_performance_model.getCalibratedTrimAirspeed());
_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;
// 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;
_performance_model.runSanityChecks();
}
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(load_factor);
_tecs.set_load_factor(getLoadFactor());
}
}
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()) {
_airspeed_slew_rate_controller.setForcedValue(_param_fw_airspd_max.get());
if (_airspeed_slew_rate_controller.getState() > _performance_model.getMaximumCalibratedAirspeed()) {
_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();
float adjusted_min_airspeed = _param_fw_airspd_min.get();
_performance_model.getMinimumCalibratedAirspeed(getLoadFactor());
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();
float adjusted_min_airspeed = _param_fw_airspd_min.get();
_performance_model.getMinimumCalibratedAirspeed(getLoadFactor());
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(),
_param_fw_airspd_max.get());
_airspeed_after_transition = constrain(_airspeed_after_transition,
_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,
throttle_max, airspeed_sp);
const float throttle_trim_compensated = _performance_model.getTrimThrottle(throttle_min,
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_adjusted,
throttle_trim_compensated,
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[])
{

View File

@ -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,

View File

@ -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);

View File

@ -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.
*

View File

@ -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();

View File

@ -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);

View File

@ -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]

View File

@ -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};

View File

@ -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;
}

View File

@ -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',