From a81515f50be4b6003baf75e4c3de5e23cdc882cd Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Tue, 11 May 2021 15:02:40 +0300 Subject: [PATCH] FixedWingPositionControl: control only height rate when using pitch stick in manual altitude controlled modes Signed-off-by: RomanBapst --- .../FixedwingPositionControl.cpp | 23 ++++++++++++++----- .../FixedwingPositionControl.hpp | 1 + 2 files changed, 18 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index d0ca157051..edda956186 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -641,14 +641,22 @@ FixedwingPositionControl::update_desired_altitude(float dt) if (_manual_control_setpoint_altitude > deadBand) { /* pitching down */ float pitch = -(_manual_control_setpoint_altitude - deadBand) / factor; - _hold_alt += (_param_sinkrate_target.get() * dt) * pitch; - _was_in_deadband = false; + + if (pitch * _param_sinkrate_target.get() < _tecs.hgt_rate_setpoint()) { + _hold_alt += (_param_sinkrate_target.get() * dt) * pitch; + _manual_height_rate_setpoint_m_s = pitch * _param_sinkrate_target.get(); + _was_in_deadband = false; + } } else if (_manual_control_setpoint_altitude < - deadBand) { /* pitching up */ float pitch = -(_manual_control_setpoint_altitude + deadBand) / factor; - _hold_alt += (_param_climbrate_target.get() * dt) * pitch; - _was_in_deadband = false; + + if (pitch * _param_climbrate_target.get() > _tecs.hgt_rate_setpoint()) { + _hold_alt += (_param_climbrate_target.get() * dt) * pitch; + _manual_height_rate_setpoint_m_s = pitch * _param_climbrate_target.get(); + _was_in_deadband = false; + } } else if (!_was_in_deadband) { /* store altitude at which manual.x was inside deadBand @@ -657,6 +665,7 @@ FixedwingPositionControl::update_desired_altitude(float dt) _hold_alt = _current_altitude; _althold_epv = _local_pos.epv; _was_in_deadband = true; + _manual_height_rate_setpoint_m_s = NAN; } if (_vehicle_status.is_vtol) { @@ -1113,7 +1122,8 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 _param_fw_thr_cruise.get(), false, pitch_limit_min, - tecs_status_s::TECS_MODE_NORMAL); + tecs_status_s::TECS_MODE_NORMAL, + _manual_height_rate_setpoint_m_s); /* heading control */ if (fabsf(_manual_control_setpoint.y) < HDG_HOLD_MAN_INPUT_THRESH && @@ -1225,7 +1235,8 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 _param_fw_thr_cruise.get(), false, pitch_limit_min, - tecs_status_s::TECS_MODE_NORMAL); + tecs_status_s::TECS_MODE_NORMAL, + _manual_height_rate_setpoint_m_s); _att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get()); _att_sp.yaw_body = 0; diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index e051facedd..0d953ace7a 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -179,6 +179,7 @@ private: float _global_local_alt0{NAN}; float _hold_alt{0.0f}; ///< hold altitude for altitude mode + float _manual_height_rate_setpoint_m_s{NAN}; float _takeoff_ground_alt{0.0f}; ///< ground altitude at which plane was launched float _hdg_hold_yaw{0.0f}; ///< hold heading for velocity mode bool _hdg_hold_enabled{false}; ///< heading hold enabled