From ec334f7c9b2b25430f3d92e3a57381774c850327 Mon Sep 17 00:00:00 2001 From: Roman Date: Mon, 18 Jul 2016 15:22:09 +0200 Subject: [PATCH] fw_pos_control_l1: limit max roll and pitch setpoints for manual modes Signed-off-by: Roman --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 90879d9811..699f6e1be1 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -2230,6 +2230,11 @@ FixedwingPositionControl::task_main() _att_sp.roll_body += _parameters.rollsp_offset_rad; _att_sp.pitch_body += _parameters.pitchsp_offset_rad; + if (_control_mode.flag_control_manual_enabled) { + _att_sp.roll_body = math::constrain(_att_sp.roll_body, -_parameters.man_roll_max_rad, _parameters.man_roll_max_rad); + _att_sp.pitch_body = math::constrain(_att_sp.pitch_body, -_parameters.man_pitch_max_rad, _parameters.man_pitch_max_rad); + } + /* lazily publish the setpoint only once available */ if (_attitude_sp_pub != nullptr) { /* publish the attitude setpoint */