diff --git a/msg/VehicleAirData.msg b/msg/VehicleAirData.msg index c1e55c1631..59ca5e5c8d 100644 --- a/msg/VehicleAirData.msg +++ b/msg/VehicleAirData.msg @@ -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. diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 8c5b1c7364..fa3b14edb8 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -44,6 +44,7 @@ */ #include "batt_smbus.h" +#include 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) { diff --git a/src/drivers/smart_battery/batmon/batmon.cpp b/src/drivers/smart_battery/batmon/batmon.cpp index e3e54db2f2..bb6b61da0b 100644 --- a/src/drivers/smart_battery/batmon/batmon.cpp +++ b/src/drivers/smart_battery/batmon/batmon.cpp @@ -43,6 +43,7 @@ #include "batmon.h" #include +#include 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) { diff --git a/src/drivers/uavcan/sensors/airspeed.cpp b/src/drivers/uavcan/sensors/airspeed.cpp index 1757c3a145..e62b29d5ea 100644 --- a/src/drivers/uavcan/sensors/airspeed.cpp +++ b/src/drivers/uavcan/sensors/airspeed.cpp @@ -38,7 +38,7 @@ #include #include "airspeed.hpp" #include -#include // For CONSTANTS_* +#include 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); } diff --git a/src/drivers/uavcan/sensors/baro.cpp b/src/drivers/uavcan/sensors/baro.cpp index 6658d4090c..90adfe1759 100644 --- a/src/drivers/uavcan/sensors/baro.cpp +++ b/src/drivers/uavcan/sensors/baro.cpp @@ -41,7 +41,7 @@ #include #include -#include // For CONSTANTS_* +#include 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; diff --git a/src/drivers/uavcan/sensors/battery.cpp b/src/drivers/uavcan/sensors/battery.cpp index bace756c65..ab09c32740 100644 --- a/src/drivers/uavcan/sensors/battery.cpp +++ b/src/drivers/uavcan/sensors/battery.cpp @@ -33,7 +33,7 @@ #include "battery.hpp" -#include +#include #include #include @@ -116,7 +116,7 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure= 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::ReceivedDataStructuregetBatteryStatus(); - _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 diff --git a/src/drivers/uavcan/sensors/differential_pressure.cpp b/src/drivers/uavcan/sensors/differential_pressure.cpp index 95d87f01a2..2457787348 100644 --- a/src/drivers/uavcan/sensors/differential_pressure.cpp +++ b/src/drivers/uavcan/sensors/differential_pressure.cpp @@ -38,7 +38,7 @@ #include "differential_pressure.hpp" #include -#include +#include #include #include @@ -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; diff --git a/src/drivers/uavcan/sensors/hygrometer.cpp b/src/drivers/uavcan/sensors/hygrometer.cpp index e22ce31d24..98bd4b1155 100755 --- a/src/drivers/uavcan/sensors/hygrometer.cpp +++ b/src/drivers/uavcan/sensors/hygrometer.cpp @@ -34,7 +34,7 @@ #include "hygrometer.hpp" #include -#include +#include const char *const UavcanHygrometerBridge::NAME = "hygrometer_sensor"; @@ -65,7 +65,7 @@ void UavcanHygrometerBridge::hygro_sub_cb(const uavcan::ReceivedDataStructure #include +#include 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; diff --git a/src/drivers/uavcannode/Publishers/RawAirData.hpp b/src/drivers/uavcannode/Publishers/RawAirData.hpp index c711213f7a..6226a5feb4 100644 --- a/src/drivers/uavcannode/Publishers/RawAirData.hpp +++ b/src/drivers/uavcannode/Publishers/RawAirData.hpp @@ -39,6 +39,7 @@ #include #include +#include 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::broadcast(raw_air_data); diff --git a/src/drivers/uavcannode/Publishers/StaticTemperature.hpp b/src/drivers/uavcannode/Publishers/StaticTemperature.hpp index 46998967f0..3784c4b067 100644 --- a/src/drivers/uavcannode/Publishers/StaticTemperature.hpp +++ b/src/drivers/uavcannode/Publishers/StaticTemperature.hpp @@ -39,6 +39,7 @@ #include #include +#include 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::broadcast(static_temperature); // ensure callback is registered diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 24e680aba0..1d88b3b022 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -61,6 +61,7 @@ add_subdirectory(mixer_module EXCLUDE_FROM_ALL) add_subdirectory(motion_planning EXCLUDE_FROM_ALL) add_subdirectory(npfg EXCLUDE_FROM_ALL) add_subdirectory(perf EXCLUDE_FROM_ALL) +add_subdirectory(fw_performance_model EXCLUDE_FROM_ALL) add_subdirectory(pid EXCLUDE_FROM_ALL) add_subdirectory(pid_design EXCLUDE_FROM_ALL) add_subdirectory(rate_control EXCLUDE_FROM_ALL) diff --git a/src/lib/airspeed/airspeed.cpp b/src/lib/airspeed/airspeed.cpp index 4d29be4a00..b563ecd3b5 100644 --- a/src/lib/airspeed/airspeed.cpp +++ b/src/lib/airspeed/airspeed.cpp @@ -43,10 +43,10 @@ #include "airspeed.h" #include -#include #include 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); } diff --git a/src/lib/atmosphere/atmosphere.cpp b/src/lib/atmosphere/atmosphere.cpp index 66b98626f7..5fc22ddf7c 100644 --- a/src/lib/atmosphere/atmosphere.cpp +++ b/src/lib/atmosphere/atmosphere.cpp @@ -41,19 +41,16 @@ namespace atmosphere { -static constexpr float kTempRefKelvin = 15.f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // temperature at base height in Kelvin -static constexpr float kTempGradient = -6.5f / 1000.f; // temperature gradient in degrees per meter -static constexpr float kPressRefSeaLevelPa = 101325.f; // pressure at sea level in 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; } diff --git a/src/lib/atmosphere/atmosphere.h b/src/lib/atmosphere/atmosphere.h index 849a430347..eeca58422d 100644 --- a/src/lib/atmosphere/atmosphere.h +++ b/src/lib/atmosphere/atmosphere.h @@ -44,6 +44,17 @@ namespace atmosphere // NOTE: We are currently only modelling the first layer of the US Standard Atmosphere 1976. // This means that the functions are only valid up to an altitude of 11km. +static constexpr float kAirDensitySeaLevelStandardAtmos = 1.225f; // kg/m^3 + +// [kg/m^3] air density of standard atmosphere at 11000m above mean sea level (this is the upper limit for the standard +// atmosphere model we are using, see atmosphere lib used) +static constexpr float kAirDensityStandardAtmos11000Amsl = 0.3639; + +static constexpr float kAirGasConstant = 287.1f; // J/(kg * K) +static constexpr float 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. diff --git a/src/lib/drivers/smbus_sbs/SBS.hpp b/src/lib/drivers/smbus_sbs/SBS.hpp index 176928923a..fcb8c614ce 100644 --- a/src/lib/drivers/smbus_sbs/SBS.hpp +++ b/src/lib/drivers/smbus_sbs/SBS.hpp @@ -47,8 +47,7 @@ #include #include #include -#include - +#include using namespace time_literals; @@ -292,7 +291,7 @@ int SMBUS_SBS_BaseClass::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; diff --git a/src/lib/fw_performance_model/CMakeLists.txt b/src/lib/fw_performance_model/CMakeLists.txt new file mode 100644 index 0000000000..a09424fdee --- /dev/null +++ b/src/lib/fw_performance_model/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2023 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(performance_model + PerformanceModel.cpp +) diff --git a/src/lib/fw_performance_model/PerformanceModel.cpp b/src/lib/fw_performance_model/PerformanceModel.cpp new file mode 100644 index 0000000000..6fb315cb89 --- /dev/null +++ b/src/lib/fw_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 +#include +#include "PerformanceModel.hpp" +#include + +using namespace atmosphere; + + +// [.] minimum ratio between the actual vehicle weight and the vehicle nominal weight (weight at which the performance limits are derived) +static constexpr float kMinWeightRatio = 0.5f; + +// [.] maximum ratio between the actual vehicle weight and the vehicle nominal weight (weight at which the performance limits are derived) +static constexpr float kMaxWeightRatio = 2.0f; + +// [m/s] climbrate defining the service ceiling, used to compensate max climbrate based on air density +static constexpr float kClimbRateAtServiceCeiling = 0.5f; + +PerformanceModel::PerformanceModel(): ModuleParams(nullptr) +{ + updateParams(); +} +float PerformanceModel::getWeightRatio() const +{ + float weight_factor = 1.0f; + + if (_param_weight_base.get() > FLT_EPSILON && _param_weight_gross.get() > FLT_EPSILON) { + weight_factor = math::constrain(_param_weight_gross.get() / _param_weight_base.get(), kMinWeightRatio, + kMaxWeightRatio); + } + + return weight_factor; +} +float PerformanceModel::getMaximumClimbRate(float air_density) const +{ + air_density = sanitiseAirDensity(air_density); + float climbrate_max = _param_fw_t_clmb_max.get(); + + const float service_ceiling = _param_service_ceiling.get(); + + if (service_ceiling > FLT_EPSILON) { + const float ceiling_pressure = getPressureFromAltitude(service_ceiling); + const float ceiling_density = getDensityFromPressureAndTemp(ceiling_pressure, + getStandardTemperatureAtAltitude(service_ceiling)); + const float climbrate_gradient = math::max((_param_fw_t_clmb_max.get() - kClimbRateAtServiceCeiling) / + (kAirDensitySeaLevelStandardAtmos - + ceiling_density), 0.0f); + const float delta_rho = air_density - kAirDensitySeaLevelStandardAtmos; + climbrate_max = math::constrain(_param_fw_t_clmb_max.get() + climbrate_gradient * delta_rho, kClimbRateAtServiceCeiling, + _param_fw_t_clmb_max.get()); + } + + return climbrate_max / getWeightRatio(); +} +float PerformanceModel::getTrimThrottle(float throttle_min, float throttle_max, float airspeed_sp, + float air_density) const +{ + const float throttle_trim = getTrimThrottleForCalibratedAirspeed(airspeed_sp) * getAirDensityThrottleScale( + air_density); + return math::constrain(throttle_trim, throttle_min, throttle_max); +} + +float PerformanceModel::getAirDensityThrottleScale(float air_density) const +{ + air_density = sanitiseAirDensity(air_density); + float air_density_throttle_scale = 1.0f; + + // scale throttle as a function of sqrt(rho0/rho) + const float eas2tas = sqrtf(kAirDensitySeaLevelStandardAtmos / air_density); + const float eas2tas_at_11km_amsl = sqrtf(kAirDensitySeaLevelStandardAtmos / kAirDensityStandardAtmos11000Amsl); + air_density_throttle_scale = math::constrain(eas2tas, 1.f, eas2tas_at_11km_amsl); + + return air_density_throttle_scale; +} +float PerformanceModel::getTrimThrottleForCalibratedAirspeed(float calibrated_airspeed_sp) const +{ + float throttle_trim = + _param_fw_thr_trim.get(); // throttle required for level flight at trim airspeed, at sea level (standard atmosphere) + + // Drag modelling (parasite drag): calculate mapping airspeed-->throttle, assuming a linear relation with different gradients +// above and below trim. This is tunable thorugh FW_THR_ASPD_MIN and FW_THR_ASPD_MAX. + const float slope_below_trim = (_param_fw_thr_trim.get() - _param_fw_thr_aspd_min.get()) / + (_param_fw_airspd_trim.get() - _param_fw_airspd_min.get()); + const float slope_above_trim = (_param_fw_thr_aspd_max.get() - _param_fw_thr_trim.get()) / + (_param_fw_airspd_max.get() - _param_fw_airspd_trim.get()); + + if (PX4_ISFINITE(calibrated_airspeed_sp) && PX4_ISFINITE(slope_below_trim) && _param_fw_thr_aspd_min.get() > FLT_EPSILON + && calibrated_airspeed_sp < _param_fw_airspd_trim.get()) { + throttle_trim = _param_fw_thr_trim.get() - slope_below_trim * (_param_fw_airspd_trim.get() - calibrated_airspeed_sp); + + } else if (PX4_ISFINITE(calibrated_airspeed_sp) && PX4_ISFINITE(slope_above_trim) + && _param_fw_thr_aspd_max.get() > FLT_EPSILON + && calibrated_airspeed_sp > _param_fw_airspd_trim.get()) { + throttle_trim = _param_fw_thr_trim.get() + slope_above_trim * (calibrated_airspeed_sp - _param_fw_airspd_trim.get()); + } + + return throttle_trim; +} +float PerformanceModel::getMinimumSinkRate(float air_density) const +{ + air_density = sanitiseAirDensity(air_density); + return _param_fw_t_sink_min.get() * sqrtf(getWeightRatio() * kAirDensitySeaLevelStandardAtmos / air_density); +} +float PerformanceModel::getCalibratedTrimAirspeed() const +{ + return math::constrain(_param_fw_airspd_trim.get() * sqrtf(getWeightRatio()), _param_fw_airspd_min.get(), + _param_fw_airspd_max.get()); +} +float PerformanceModel::getMinimumCalibratedAirspeed(float load_factor) const +{ + + load_factor = math::max(load_factor, FLT_EPSILON); + return _param_fw_airspd_min.get() * sqrtf(getWeightRatio() * load_factor); +} + +float PerformanceModel::getCalibratedStallAirspeed(float load_factor) const +{ + load_factor = math::max(load_factor, FLT_EPSILON); + return _param_fw_airspd_stall.get() * sqrtf(getWeightRatio() * load_factor); +} + +float PerformanceModel::getMaximumCalibratedAirspeed() const +{ + return _param_fw_airspd_max.get(); +} +bool PerformanceModel::runSanityChecks() const +{ + bool ret = true; + + // sanity check parameters + if (_param_fw_airspd_max.get() < _param_fw_airspd_min.get()) { + /* EVENT + * @description + * - FW_AIRSPD_MAX: {1:.1} + * - FW_AIRSPD_MIN: {2:.1} + */ + events::send(events::ID("fixedwing_position_control_conf_invalid_airspeed"), events::Log::Error, + "Invalid configuration: Airspeed max smaller than min", + _param_fw_airspd_max.get(), _param_fw_airspd_min.get()); + ret = false; + } + + if (_param_fw_airspd_max.get() < 5.0f || _param_fw_airspd_min.get() > 100.0f) { + /* EVENT + * @description + * - FW_AIRSPD_MAX: {1:.1} + * - FW_AIRSPD_MIN: {2:.1} + */ + events::send(events::ID("fixedwing_position_control_conf_invalid_airspeed_bounds"), events::Log::Error, + "Invalid configuration: Airspeed max \\< 5 m/s or min \\> 100 m/s", + _param_fw_airspd_max.get(), _param_fw_airspd_min.get()); + ret = false; + } + + if (_param_fw_airspd_trim.get() < _param_fw_airspd_min.get() || + _param_fw_airspd_trim.get() > _param_fw_airspd_max.get()) { + /* EVENT + * @description + * - FW_AIRSPD_MAX: {1:.1} + * - FW_AIRSPD_MIN: {2:.1} + * - FW_AIRSPD_TRIM: {3:.1} + */ + events::send(events::ID("fixedwing_position_control_conf_invalid_trim_bounds"), + events::Log::Error, + "Invalid configuration: Airspeed trim out of min or max bounds", + _param_fw_airspd_max.get(), _param_fw_airspd_min.get(), _param_fw_airspd_trim.get()); + ret = false; + } + + if (_param_fw_airspd_stall.get() > _param_fw_airspd_min.get()) { + /* EVENT + * @description + * - FW_AIRSPD_MIN: {1:.1} + * - FW_AIRSPD_STALL: {2:.1} + */ + events::send(events::ID("fixedwing_position_control_conf_invalid_stall"), events::Log::Error, + "Invalid configuration: FW_AIRSPD_STALL higher FW_AIRSPD_MIN", + _param_fw_airspd_min.get(), _param_fw_airspd_stall.get()); + ret = false; + } + + return ret; + +} +void PerformanceModel:: updateParameters() +{ + updateParams(); +} + +float PerformanceModel::sanitiseAirDensity(float air_density) +{ + if (!PX4_ISFINITE(air_density)) { + air_density = kAirDensitySeaLevelStandardAtmos; + } + + return math::max(air_density, kAirDensityStandardAtmos11000Amsl); +} diff --git a/src/lib/fw_performance_model/PerformanceModel.hpp b/src/lib/fw_performance_model/PerformanceModel.hpp new file mode 100644 index 0000000000..c283fd4d01 --- /dev/null +++ b/src/lib/fw_performance_model/PerformanceModel.hpp @@ -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 + +#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) _param_fw_airspd_max, + (ParamFloat) _param_fw_airspd_min, + (ParamFloat) _param_fw_airspd_trim, + (ParamFloat) _param_fw_airspd_stall, + (ParamFloat) _param_fw_t_clmb_max, + (ParamFloat) _param_fw_t_sink_min, + (ParamFloat) _param_weight_base, + (ParamFloat) _param_weight_gross, + (ParamFloat) _param_service_ceiling, + (ParamFloat) _param_fw_thr_trim, + (ParamFloat) _param_fw_thr_idle, + (ParamFloat) _param_fw_thr_max, + (ParamFloat) _param_fw_thr_min, + (ParamFloat) _param_fw_thr_aspd_min, + (ParamFloat) _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_ diff --git a/src/lib/fw_performance_model/performance_model_params.c b/src/lib/fw_performance_model/performance_model_params.c new file mode 100644 index 0000000000..0c39d2ed39 --- /dev/null +++ b/src/lib/fw_performance_model/performance_model_params.c @@ -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); diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 8d38bd2632..30cc10b6fe 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -54,19 +54,8 @@ #include static constexpr float CONSTANTS_ONE_G = 9.80665f; // m/s^2 - -static constexpr float CONSTANTS_STD_PRESSURE_PA = 101325.0f; // pascals (Pa) -static constexpr float CONSTANTS_STD_PRESSURE_KPA = CONSTANTS_STD_PRESSURE_PA / 1000.0f; // kilopascals (kPa) -static constexpr float CONSTANTS_STD_PRESSURE_MBAR = CONSTANTS_STD_PRESSURE_PA / - 100.0f; // Millibar (mbar) (1 mbar = 100 Pa) - -static constexpr float CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C = 1.225f; // kg/m^3 -static constexpr float CONSTANTS_AIR_GAS_CONST = 287.1f; // J/(kg * K) -static constexpr float CONSTANTS_ABSOLUTE_NULL_CELSIUS = -273.15f; // °C - static constexpr double CONSTANTS_RADIUS_OF_EARTH = 6371000; // meters (m) static constexpr float CONSTANTS_RADIUS_OF_EARTH_F = CONSTANTS_RADIUS_OF_EARTH; // meters (m) - static constexpr float CONSTANTS_EARTH_SPIN_RATE = 7.2921150e-5f; // radians/second (rad/s) diff --git a/src/lib/parameters/px4params/srcparser.py b/src/lib/parameters/px4params/srcparser.py index f60789ea1b..93aca16358 100644 --- a/src/lib/parameters/px4params/srcparser.py +++ b/src/lib/parameters/px4params/srcparser.py @@ -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', diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 71bdbbe3ce..5d48788945 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -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; diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index 49b4a72ede..f41a9f4611 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -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, diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index 651fbcb680..5dc2a11a7c 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -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) diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp index 2a027920a2..ab5267d2d6 100644 --- a/src/modules/commander/baro_calibration.cpp +++ b/src/modules/commander/baro_calibration.cpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include #include #include @@ -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; diff --git a/src/modules/ekf2/EKF/drag_fusion.cpp b/src/modules/ekf2/EKF/drag_fusion.cpp index a9273a597b..287bfb69f9 100644 --- a/src/modules/ekf2/EKF/drag_fusion.cpp +++ b/src/modules/ekf2/EKF/drag_fusion.cpp @@ -41,6 +41,7 @@ #include #include +#include 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; diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 5143a10c2d..4d4cae818b 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -71,7 +71,7 @@ # include "sensor_range_finder.hpp" #endif // CONFIG_EKF2_RANGE_FINDER -#include +#include #include #include #include @@ -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 diff --git a/src/modules/ekf2/test/test_EKF_drag_fusion.cpp b/src/modules/ekf2/test/test_EKF_drag_fusion.cpp index 7e4bc4eda2..ca03a0f5e7 100644 --- a/src/modules/ekf2/test/test_EKF_drag_fusion.cpp +++ b/src/modules/ekf2/test/test_EKF_drag_fusion.cpp @@ -39,6 +39,7 @@ #include "EKF/ekf.h" #include "sensor_simulator/sensor_simulator.h" #include "sensor_simulator/ekf_wrapper.h" +#include 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 diff --git a/src/modules/fw_pos_control/CMakeLists.txt b/src/modules/fw_pos_control/CMakeLists.txt index 5da7f5618c..9248e8d2df 100644 --- a/src/modules/fw_pos_control/CMakeLists.txt +++ b/src/modules/fw_pos_control/CMakeLists.txt @@ -41,6 +41,7 @@ set(POSCONTROL_DEPENDENCIES SlewRate tecs motion_planning + performance_model ) if(CONFIG_FIGURE_OF_EIGHT) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index e9b88ba70f..5cd3a76272 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -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 - * - FW_AIRSPD_MAX: {1:.1} - * - FW_AIRSPD_MIN: {2:.1} - */ - events::send(events::ID("fixedwing_position_control_conf_invalid_airspeed"), events::Log::Error, - "Invalid configuration: Airspeed max smaller than min", - _param_fw_airspd_max.get(), _param_fw_airspd_min.get()); - check_ret = PX4_ERROR; - } - - if (_param_fw_airspd_max.get() < 5.0f || _param_fw_airspd_min.get() > 100.0f) { - /* EVENT - * @description - * - FW_AIRSPD_MAX: {1:.1} - * - FW_AIRSPD_MIN: {2:.1} - */ - events::send(events::ID("fixedwing_position_control_conf_invalid_airspeed_bounds"), events::Log::Error, - "Invalid configuration: Airspeed max \\< 5 m/s or min \\> 100 m/s", - _param_fw_airspd_max.get(), _param_fw_airspd_min.get()); - 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 - * - FW_AIRSPD_MAX: {1:.1} - * - FW_AIRSPD_MIN: {2:.1} - * - FW_AIRSPD_TRIM: {3:.1} - */ - events::send(events::ID("fixedwing_position_control_conf_invalid_trim_bounds"), - events::Log::Error, - "Invalid configuration: Airspeed trim out of min or max bounds", - _param_fw_airspd_max.get(), _param_fw_airspd_min.get(), _param_fw_airspd_trim.get()); - check_ret = PX4_ERROR; - } - - if (_param_fw_airspd_stall.get() > _param_fw_airspd_min.get()) { - /* EVENT - * @description - * - FW_AIRSPD_MIN: {1:.1} - * - FW_AIRSPD_STALL: {2:.1} - */ - events::send(events::ID("fixedwing_position_control_conf_invalid_stall"), events::Log::Error, - "Invalid configuration: FW_AIRSPD_STALL higher FW_AIRSPD_MIN", - _param_fw_airspd_min.get(), _param_fw_airspd_stall.get()); - 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[]) { diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index 5d448b213f..dc252fea30 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -49,11 +49,13 @@ #include "launchdetection/LaunchDetector.h" #include "runway_takeoff/RunwayTakeoff.h" +#include #include #include #include +#include #include #include #include @@ -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 ¤t_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 _airspeed_slew_rate_controller; @@ -909,12 +894,6 @@ private: const matrix::Vector2f &wind_vel); DEFINE_PARAMETERS( - - (ParamFloat) _param_fw_airspd_max, - (ParamFloat) _param_fw_airspd_min, - (ParamFloat) _param_fw_airspd_trim, - (ParamFloat) _param_fw_airspd_stall, - (ParamFloat) _param_fw_gnd_spd_min, (ParamFloat) _param_fw_pn_r_slew_max, @@ -944,7 +923,6 @@ private: (ParamFloat) _param_fw_p_lim_max, (ParamFloat) _param_fw_p_lim_min, - (ParamFloat) _param_fw_t_clmb_max, (ParamFloat) _param_fw_t_hrate_ff, (ParamFloat) _param_fw_t_h_error_tc, (ParamFloat) _param_fw_t_I_gain_thr, @@ -952,7 +930,6 @@ private: (ParamFloat) _param_fw_t_ptch_damp, (ParamFloat) _param_fw_t_rll2thr, (ParamFloat) _param_fw_t_sink_max, - (ParamFloat) _param_fw_t_sink_min, (ParamFloat) _param_fw_t_spdweight, (ParamFloat) _param_fw_t_tas_error_tc, (ParamFloat) _param_fw_t_thr_damp, @@ -965,10 +942,6 @@ private: (ParamFloat) _param_speed_rate_standard_dev, (ParamFloat) _param_process_noise_standard_dev, - (ParamFloat) _param_fw_thr_aspd_min, - (ParamFloat) _param_fw_thr_aspd_max, - - (ParamFloat) _param_fw_thr_trim, (ParamFloat) _param_fw_thr_idle, (ParamFloat) _param_fw_thr_max, (ParamFloat) _param_fw_thr_min, @@ -995,9 +968,6 @@ private: (ParamFloat) _param_nav_fw_alt_rad, - (ParamFloat) _param_weight_base, - (ParamFloat) _param_weight_gross, - (ParamFloat) _param_fw_wing_span, (ParamFloat) _param_fw_wing_height, diff --git a/src/modules/fw_pos_control/fw_path_navigation_params.c b/src/modules/fw_pos_control/fw_path_navigation_params.c index af04e328cf..03243340fd 100644 --- a/src/modules/fw_pos_control/fw_path_navigation_params.c +++ b/src/modules/fw_pos_control/fw_path_navigation_params.c @@ -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); diff --git a/src/modules/fw_rate_control/fw_rate_control_params.c b/src/modules/fw_rate_control/fw_rate_control_params.c index cffd790401..ee6e194ce3 100644 --- a/src/modules/fw_rate_control/fw_rate_control_params.c +++ b/src/modules/fw_rate_control/fw_rate_control_params.c @@ -40,37 +40,6 @@ * @author Thomas Gubler */ -/** - * 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. * diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index 4c15c74a2a..d8654c4071 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -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(); diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 7b94bd63bd..8c60cd4d5d 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -35,7 +35,7 @@ #include -#include +#include #include #include @@ -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(air_speed_value); // hPa to Pa; - report.temperature = static_cast(air_speed.temperature()) + CONSTANTS_ABSOLUTE_NULL_CELSIUS; // K to C + report.temperature = static_cast(air_speed.temperature()) + atmosphere::kAbsoluteNullCelsius; // K to C report.timestamp = hrt_absolute_time();; _differential_pressure_pub.publish(report); diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index e77b7d15e2..a4227978be 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -61,7 +61,7 @@ #include // matrix, vectors, dcm, quaterions #include // math::radians, -#include // to get the physical constants +#include // to get the physical constants #include // to get the real time #include #include @@ -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] diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 94044b3384..132448177f 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -50,7 +50,7 @@ #pragma once #include -#include +#include #include #include #include @@ -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}; diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index ea66ea5348..bf1ca82a9a 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -45,6 +45,7 @@ #include #include #include +#include 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; } diff --git a/validation/module_schema.yaml b/validation/module_schema.yaml index 6d733a6e51..273024fc18 100644 --- a/validation/module_schema.yaml +++ b/validation/module_schema.yaml @@ -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',