fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 13:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

FWPositionControl: trim throttle calc: guard against min/max=trim airspeed (#21469)

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-04-14 11:55:23 +02:00
committed by GitHub
parent bc560ddddb
commit da519573d4
1 changed files with 11 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control/FixedwingPositionControl.cpp
+11 -8
View File
@@ -2466,17 +2466,20 @@ float FixedwingPositionControl::calculateTrimThrottle(float throttle_min,
float throttle_trim = float throttle_trim =
_param_fw_thr_trim.get(); // throttle required for level flight at trim airspeed, at sea level (standard atmosphere) _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 gradient // 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.
if (PX4_ISFINITE(airspeed_sp) && _param_fw_thr_aspd_min.get() > FLT_EPSILON const float slope_below_trim = (_param_fw_thr_trim.get() - _param_fw_thr_aspd_min.get()) /
&& airspeed_sp < _param_fw_airspd_trim.get()) { (_param_fw_airspd_trim.get() - _param_fw_airspd_min.get());
throttle_trim = _param_fw_thr_trim.get() - (_param_fw_thr_trim.get() - _param_fw_thr_aspd_min.get()) / const float slope_above_trim = (_param_fw_thr_aspd_max.get() - _param_fw_thr_trim.get()) /
(_param_fw_airspd_trim.get() - _param_fw_airspd_min.get()) * (_param_fw_airspd_trim.get() - airspeed_sp); (_param_fw_airspd_max.get() - _param_fw_airspd_trim.get());
} else if (PX4_ISFINITE(airspeed_sp) && _param_fw_thr_aspd_max.get() > FLT_EPSILON 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()) { && airspeed_sp > _param_fw_airspd_trim.get()) {
throttle_trim = _param_fw_thr_trim.get() + (_param_fw_thr_aspd_max.get() - _param_fw_thr_trim.get()) / throttle_trim = _param_fw_thr_trim.get() + slope_above_trim * (airspeed_sp - _param_fw_airspd_trim.get());
(_param_fw_airspd_max.get() - _param_fw_airspd_trim.get()) * (airspeed_sp - _param_fw_airspd_trim.get());
} }
float weight_ratio = 1.0f; float weight_ratio = 1.0f;