dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

FixedWingPositionControl: compensate minimum sink rate for weight and density 

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst 2023-09-06 14:02:15 +03:00
parent f847586b10
commit d6a770a547
2 changed files with 30 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/modules/fw_pos_control/FixedwingPositionControl.cpp
View File

@ -94,16 +94,21 @@ FixedwingPositionControl::init()
return true;
}
float FixedwingPositionControl::getMaximumClimbRate()
float FixedwingPositionControl::getWeightRatio()
{
float weight_factor = 1.0f;
if (_param_weight_base.get() > 0.0f && _param_weight_gross.get() > 0.0f) {
weight_factor = math::constrain(_param_weight_base.get() / _param_weight_gross.get(), MIN_WEIGHT_RATIO,
weight_factor = math::constrain(_param_weight_gross.get() / _param_weight_base.get(), MIN_WEIGHT_RATIO,
MAX_WEIGHT_RATIO);
}
return weight_factor;
}
float FixedwingPositionControl::getMaximumClimbRate()
{
float climbrate_max = _param_fw_t_clmb_max.get();
const float density_min = _param_density_min.get();
@ -116,7 +121,12 @@ float FixedwingPositionControl::getMaximumClimbRate()
climbrate_max = _param_fw_t_clmb_max.get() + density_gradient * delta_rho;
}
return climbrate_max * weight_factor;
return climbrate_max / getWeightRatio();
}
float FixedwingPositionControl::getMinimumSinkRate()
{
return _param_fw_t_sink_min.get() * sqrtf(getWeightRatio() * CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / _air_density);
}
int

17
src/modules/fw_pos_control/FixedwingPositionControl.hpp
View File

@ -488,7 +488,24 @@ private:
*/
float get_terrain_altitude_takeoff(float takeoff_alt);
/**
* @brief Return the maximum climb rate achievable under the current conditions.
* @return Maximum climbrate.
*/
float getMaximumClimbRate();
/**
* @brief Return the minimum sink rate achievable under the current conditions.
* @return Minimum sink rate.
*/
float getMinimumSinkRate();
/**
* @brief Return the ratio of actual vehicle weight to vehicle base weight.
* @return Weight ratio.
*/
float getWeightRatio();
/**
* @brief Maps the manual control setpoint (pilot sticks) to height rate commands
*