From 41b0a6c62c7617515e8dd11aedcc4825b661e546 Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Tue, 14 Jun 2022 21:00:56 -0500 Subject: [PATCH] fw pos ctrl: open up desired max sink rate limits for landing slope - the target_sink_rate param could possibly constrain the maximum commanded sink rate to something less than that of the landing glide slope, which would make it impossible to track. this commit allows opening up the desired max sink rate up to the performance limits of the aircraft, if necessary, for the landing case --- .../FixedwingPositionControl.cpp | 39 +++++++++++++------ .../FixedwingPositionControl.hpp | 9 ++--- 2 files changed, 31 insertions(+), 17 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 3bb9f9a8f6..846e630d38 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -947,7 +947,8 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i _param_fw_thr_min.get(), _param_fw_thr_max.get(), false, - _param_fw_p_lim_min.get()); + _param_fw_p_lim_min.get(), + _param_sinkrate_target.get()); _att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle _att_sp.yaw_body = 0.f; @@ -981,6 +982,7 @@ FixedwingPositionControl::control_auto_descend(const float control_interval) _param_fw_thr_max.get(), false, _param_fw_p_lim_min.get(), + _param_sinkrate_target.get(), false, descend_rate); @@ -1165,7 +1167,8 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co tecs_fw_thr_min, tecs_fw_thr_max, false, - radians(_param_fw_p_lim_min.get())); + radians(_param_fw_p_lim_min.get()), + _param_sinkrate_target.get()); } void @@ -1222,6 +1225,7 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co tecs_fw_thr_max, false, radians(_param_fw_p_lim_min.get()), + _param_sinkrate_target.get(), tecs_status_s::TECS_MODE_NORMAL, pos_sp_curr.vz); } @@ -1340,7 +1344,8 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons tecs_fw_thr_min, tecs_fw_thr_max, false, - radians(_param_fw_p_lim_min.get())); + radians(_param_fw_p_lim_min.get()), + _param_sinkrate_target.get()); } void @@ -1463,7 +1468,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _param_fw_thr_min.get(), _param_fw_thr_max.get(), _runway_takeoff.climbout(), - radians(takeoff_pitch_min_climbout_deg)); + radians(takeoff_pitch_min_climbout_deg), + _param_sinkrate_target.get()); _tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); // reset after TECS calculation @@ -1558,7 +1564,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _param_fw_thr_min.get(), takeoff_throttle, true, - radians(_takeoff_pitch_min.get())); + radians(_takeoff_pitch_min.get()), + _param_sinkrate_target.get()); } else { tecs_update_pitch_throttle(control_interval, @@ -1569,7 +1576,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _param_fw_thr_min.get(), takeoff_throttle, false, - radians(_param_fw_p_lim_min.get())); + radians(_param_fw_p_lim_min.get()), + _param_sinkrate_target.get()); } if (_launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { @@ -1702,6 +1710,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo 0.0f, false, pitch_min_rad, + _param_sinkrate_target.get(), true, height_rate_setpoint); @@ -1749,6 +1758,11 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo /* longitudinal guidance */ + // open the desired max sink rate to encompass the glide slope if within the aircraft's performance limits + const float glide_slope_sink_rate = airspeed_land * sinf(math::radians(_param_fw_lnd_ang.get())); + const float desired_max_sinkrate = math::min(math::max(glide_slope_sink_rate, _param_sinkrate_target.get()), + _param_fw_t_sink_max.get()); + tecs_update_pitch_throttle(control_interval, altitude_setpoint, target_airspeed, @@ -1757,7 +1771,8 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo _param_fw_thr_min.get(), _param_fw_thr_max.get(), false, - radians(_param_fw_p_lim_min.get())); + radians(_param_fw_p_lim_min.get()), + desired_max_sinkrate); /* set the attitude and throttle commands */ @@ -1816,6 +1831,7 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval, throttle_max, false, min_pitch, + _param_sinkrate_target.get(), false, height_rate_sp); @@ -1922,6 +1938,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval, throttle_max, false, min_pitch, + _param_sinkrate_target.get(), false, height_rate_sp); @@ -2351,10 +2368,8 @@ float FixedwingPositionControl::compensateTrimThrottleForDensityAndWeight(float 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, - bool climbout_mode, float climbout_pitch_min_rad, - bool disable_underspeed_detection, float hgt_rate_sp) + float pitch_min_rad, float pitch_max_rad, float throttle_min, float throttle_max, bool climbout_mode, + float climbout_pitch_min_rad, const float desired_max_sinkrate, bool disable_underspeed_detection, float hgt_rate_sp) { _tecs_is_running = true; @@ -2435,7 +2450,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva pitch_min_rad - radians(_param_fw_psp_off.get()), pitch_max_rad - radians(_param_fw_psp_off.get()), _param_climbrate_target.get(), - _param_sinkrate_target.get(), + desired_max_sinkrate, hgt_rate_sp); tecs_status_publish(); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 5868705c4b..75fde26a00 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -662,14 +662,13 @@ private: * @param throttle_max Maximum throttle command [0,1] * @param climbout_mode True if TECS should engage climbout mode * @param climbout_pitch_min_rad Minimum pitch angle command in climbout mode [rad] + * @param desired_max_sink_rate The desired max sink rate commandable when altitude errors are large [m/s] * @param disable_underspeed_detection True if underspeed detection should be disabled * @param hgt_rate_sp Height rate setpoint [m/s] */ - void 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, - bool climbout_mode, float climbout_pitch_min_rad, - bool disable_underspeed_detection = false, float hgt_rate_sp = NAN); + void 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, bool climbout_mode, float climbout_pitch_min_rad, + const float desired_max_sink_rate, bool disable_underspeed_detection = false, float hgt_rate_sp = NAN); /** * @brief Constrains the roll angle setpoint near ground to avoid wingtip strike.