From 98c7f664c1a60799b9c2afef9786af0c48516332 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 7 Jul 2021 10:02:50 +0200 Subject: [PATCH] FW Position control: add roll slewrate also in manual Position flight mode Signed-off-by: Silvan Fuhrer --- .../fw_pos_control_l1/FixedwingPositionControl.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 52ca7a065b..c8289779d9 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1106,7 +1106,17 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 _hdg_hold_enabled = false; _yaw_lock_engaged = false; - _att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get()); + + // do slew rate limiting on roll if enabled + float roll_sp_new = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get()); + const float roll_rate_slew_rad = radians(_param_fw_l1_r_slew_max.get()); + + if (dt > 0.f && roll_rate_slew_rad > 0.f) { + roll_sp_new = constrain(roll_sp_new, _att_sp.roll_body - roll_rate_slew_rad * dt, + _att_sp.roll_body + roll_rate_slew_rad * dt); + } + + _att_sp.roll_body = roll_sp_new; _att_sp.yaw_body = 0; }