fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

compensate airspeed setpoint based on weight ratio

Browse Source 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst 2023-09-12 08:52:44 +03:00
parent f7d7a7123c
commit 1840f9645a
1 changed files with 8 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -417,13 +417,15 @@ FixedwingPositionControl::vehicle_attitude_poll()
float
FixedwingPositionControl::get_manual_airspeed_setpoint()
{
float altctrl_airspeed = _param_fw_airspd_trim.get();
float altctrl_airspeed = math::constrain(_param_fw_airspd_trim.get() * sqrtf(getWeightRatio()),
_param_fw_airspd_min.get(),
_param_fw_airspd_max.get());
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()});
{_param_fw_airspd_min.get(), altctrl_airspeed, _param_fw_airspd_max.get()});
} else if (PX4_ISFINITE(_commanded_manual_airspeed_setpoint)) {
altctrl_airspeed = _commanded_manual_airspeed_setpoint;
@ -437,7 +439,8 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
float calibrated_min_airspeed, const Vector2f &ground_speed)
{
if (!PX4_ISFINITE(calibrated_airspeed_setpoint) || calibrated_airspeed_setpoint <= FLT_EPSILON) {
calibrated_airspeed_setpoint = _param_fw_airspd_trim.get();
// increase trim airspeed based on weight ratio in order to keep constant alpha.
calibrated_airspeed_setpoint = _param_fw_airspd_trim.get() * sqrtf(getWeightRatio());
}
// Adapt cruise airspeed when otherwise the min groundspeed couldn't be maintained
@ -956,7 +959,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(),
_param_fw_airspd_trim.get() * sqrtf(getWeightRatio()),
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
_param_fw_thr_min.get(),
@ -987,7 +990,7 @@ FixedwingPositionControl::control_auto_descend(const float control_interval)
tecs_update_pitch_throttle(control_interval,
_current_altitude,
_param_fw_airspd_trim.get(),
_param_fw_airspd_trim.get() * sqrtf(getWeightRatio()),
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
_param_fw_thr_min.get(),