Skip to content

Commit

Permalink
addressed review comments
Browse files Browse the repository at this point in the history 
Signed-off-by: RomanBapst <[email protected]>
  • Loading branch information
@RomanBapst
RomanBapst committed Oct 3, 2023
1 parent 2641c1f commit 4f6dba7
Show file tree
Hide file tree
Showing 3 changed files with 110 additions and 40 deletions.
15 changes: 0 additions & 15 deletions src/modules/fw_pos_control/FixedwingPositionControl.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -142,21 +142,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;

// air density of standard athmosphere at 1000m above mean sea level [kg/m^3]
static constexpr float AIR_DENSITY_STANDARD_ATMOS_1000_AMSL = 1.112f;

// climbrate defining the service ceiling, used to compensate max climbrate based on air density
static constexpr float CLIMBRATE_MIN = 0.5f; // [m/s]

// [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;

Expand Down
62 changes: 38 additions & 24 deletions src/modules/fw_pos_control/performance_model/PerformanceModel.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,19 +42,19 @@
#include "PerformanceModel.h"

// 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;
static constexpr float kAirDensityStandardAtmos5000Amsl = 0.7363f;

// air density of standard athmosphere at 1000m above mean sea level [kg/m^3]
static constexpr float AIR_DENSITY_STANDARD_ATMOS_1000_AMSL = 1.112f;
static constexpr float kAirDensityStandardAtmos1000Amsl = 1.112f;

// [.] minimum ratio between the actual vehicle weight and the vehicle nominal weight (weight at which the performance limits are derived)
static constexpr float MIN_WEIGHT_RATIO = 0.5f;
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 MAX_WEIGHT_RATIO = 2.0f;
static constexpr float kMaxWeightRatio = 2.0f;

// climbrate defining the service ceiling, used to compensate max climbrate based on air density
static constexpr float CLIMBRATE_MIN = 0.5f; // [m/s]
static constexpr float kClimbrateMin = 0.5f; // [m/s]

PerformanceModel::PerformanceModel(): ModuleParams(nullptr)
{
Expand All @@ -65,8 +65,8 @@ 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(), MIN_WEIGHT_RATIO,
MAX_WEIGHT_RATIO);
weight_factor = math::constrain(_param_weight_gross.get() / _param_weight_base.get(), kMinWeightRatio,
kMaxWeightRatio);
}

return weight_factor;
Expand All @@ -78,10 +78,11 @@ float PerformanceModel::getMaximumClimbRate(float air_density) const

const float density_min = _param_density_min.get();

if (density_min < AIR_DENSITY_STANDARD_ATMOS_1000_AMSL
&& density_min > AIR_DENSITY_STANDARD_ATMOS_5000_AMSL) {
const float density_gradient = (_param_fw_t_clmb_max.get() - CLIMBRATE_MIN) / (CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C -
density_min);
if (density_min < kAirDensityStandardAtmos1000Amsl
&& density_min > kAirDensityStandardAtmos5000Amsl) {
const float density_gradient = math::max((_param_fw_t_clmb_max.get() - kClimbrateMin) /
(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C -
density_min), 0.0f);
const float delta_rho = air_density - CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
climbrate_max = _param_fw_t_clmb_max.get() + density_gradient * delta_rho;
}
Expand All @@ -90,12 +91,36 @@ float PerformanceModel::getMaximumClimbRate(float air_density) const
}
float PerformanceModel::getTrimThrottle(float throttle_min, float throttle_max, float airspeed_sp,
float air_density) const
{
const float throttle_trim = getTrimThrottleForAirspeed(airspeed_sp) * getAirDensityThrottleScale(
air_density) * getWeightThrottleScale();
return math::constrain(throttle_trim, throttle_min, throttle_max);
}
float PerformanceModel::getWeightThrottleScale() const
{
return sqrtf(getWeightRatio());
}

float PerformanceModel::getAirDensityThrottleScale(float air_density) const
{
float air_density_throttle_scale = 1.0f;

if (PX4_ISFINITE(air_density)) {
// scale throttle as a function of sqrt(rho0/rho)
const float eas2tas = sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / air_density);
const float eas2tas_at_5000m_amsl = sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / kAirDensityStandardAtmos5000Amsl);
air_density_throttle_scale = math::constrain(eas2tas, 1.f, eas2tas_at_5000m_amsl);
}

return air_density_throttle_scale;
}
float PerformanceModel::getTrimThrottleForAirspeed(float 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.
// 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()) /
Expand All @@ -110,18 +135,7 @@ float PerformanceModel::getTrimThrottle(float throttle_min, float throttle_max,
throttle_trim = _param_fw_thr_trim.get() + slope_above_trim * (airspeed_sp - _param_fw_airspd_trim.get());
}

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 = math::constrain(eas2tas, 1.f, eas2tas_at_5000m_amsl);
}

// compensate trim throttle for both weight and air density
return math::constrain(throttle_trim * sqrtf(getWeightRatio()) * air_density_throttle_scale, throttle_min,
throttle_max);
return throttle_trim;
}
float PerformanceModel::getMinimumSinkRate(float air_density) const
{
Expand Down
73 changes: 72 additions & 1 deletion src/modules/fw_pos_control/performance_model/PerformanceModel.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,17 +50,68 @@ class PerformanceModel : public ModuleParams

void updateParameters();

/**
* Get the maximum climb rate expected for current air density and weight.
* @param air_density in kg/m^3
* @return maximum climb rate in m/s
*/
float getMaximumClimbRate(float air_density) const;

/**
* Get the minimum sink rate expected for current air density and weight.
* @param air_density
* @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 densty and weight.
* @param throttle_min minimum throttle
* @param throttle_max maximum throttle
* @param airspeed_sp calibrated airspeed setpoint in m/s
* @param air_density air density
* @return trim throttle
*/
float getTrimThrottle(float throttle_min, float throttle_max, float airspeed_sp, float air_density) const;
float getMinimumSinkRate(float air_density) const;

/**
* Get the trim airspeed compensated for weight.
* @return calibrated trim airspeed in m/s
*/
float getTrimAirspeed() 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 getMinimumAirspeed(float load_factor = 1.0f) const;

/**
* Get the maximum airspeed.
* @return calibrated maximum airspeed in m/s
*/
float getMaximumAirspeed() 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 getStallAirspeed(float load_factor) const;

/**
* Run some checks on parameters and detect unfeasible combinations.
* @return true if all checks pass
*/
bool runSanityChecks() const;

private:
DEFINE_PARAMETERS(
(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
Expand All @@ -78,6 +129,26 @@ class PerformanceModel : public ModuleParams
(ParamFloat<px4::params::FW_THR_MIN>) _param_fw_thr_min,
(ParamFloat<px4::params::FW_THR_ASPD_MIN>) _param_fw_thr_aspd_min,
(ParamFloat<px4::params::FW_THR_ASPD_MAX>) _param_fw_thr_aspd_max)

/**
* Get the trim throttle for the current airspeed setpoint.
* @param airspeed_sp in m/s
* @return trim throttle
*/
float getTrimThrottleForAirspeed(float airspeed_sp) 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 throttle scale factor for the current weight.
* @return throttle scale factor for weight
*/
float getWeightThrottleScale() const;
};

#endif //PX4_SRC_MODULES_FW_POS_CONTROL_PERFORMANCEMODEL_H_

0 comments on commit 4f6dba7

Please sign in to comment.