From 74730273c17ad84a56459b6e11d0795e5a268dde Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Fri, 9 Mar 2018 04:55:45 +0100 Subject: [PATCH] fw_att_control move angular rate limits to body angular rates (#9023) * ecl: attitude_fw update - move angular rate limits to body angular rates - handle rattitude/acro vs attitude mode rate limits --- src/lib/ecl | 2 +- .../FixedwingAttitudeControl.cpp | 22 +++++++++++++++---- .../FixedwingAttitudeControl.hpp | 2 ++ 3 files changed, 21 insertions(+), 5 deletions(-) diff --git a/src/lib/ecl b/src/lib/ecl index 78e983073a..041886a289 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 78e983073a87c853d7e248f6f69a053af8a9e383 +Subproject commit 041886a28957c17505b19367eb8703444123a4c3 diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 84b00c9db5..483959e2d7 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -199,8 +199,6 @@ FixedwingAttitudeControl::parameters_update() _pitch_ctrl.set_k_i(_parameters.p_i); _pitch_ctrl.set_k_ff(_parameters.p_ff); _pitch_ctrl.set_integrator_max(_parameters.p_integrator_max); - _pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos)); - _pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg)); /* roll control parameters */ _roll_ctrl.set_time_constant(_parameters.r_tc); @@ -208,14 +206,12 @@ FixedwingAttitudeControl::parameters_update() _roll_ctrl.set_k_i(_parameters.r_i); _roll_ctrl.set_k_ff(_parameters.r_ff); _roll_ctrl.set_integrator_max(_parameters.r_integrator_max); - _roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax)); /* yaw control parameters */ _yaw_ctrl.set_k_p(_parameters.y_p); _yaw_ctrl.set_k_i(_parameters.y_i); _yaw_ctrl.set_k_ff(_parameters.y_ff); _yaw_ctrl.set_integrator_max(_parameters.y_integrator_max); - _yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); /* wheel control parameters */ _wheel_ctrl.set_k_p(_parameters.w_p); @@ -638,6 +634,24 @@ void FixedwingAttitudeControl::run() control_input.groundspeed = groundspeed; control_input.groundspeed_scaler = groundspeed_scaler; + /* reset body angular rate limits on mode change */ + if ((_vcontrol_mode.flag_control_attitude_enabled != _flag_control_attitude_enabled_last) || params_updated) { + if (_vcontrol_mode.flag_control_attitude_enabled) { + _roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax)); + _pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos)); + _pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg)); + _yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); + + } else { + _roll_ctrl.set_max_rate(_parameters.acro_max_x_rate_rad); + _pitch_ctrl.set_max_rate_pos(_parameters.acro_max_y_rate_rad); + _pitch_ctrl.set_max_rate_neg(_parameters.acro_max_y_rate_rad); + _yaw_ctrl.set_max_rate(_parameters.acro_max_z_rate_rad); + } + } + + _flag_control_attitude_enabled_last = _vcontrol_mode.flag_control_attitude_enabled; + /* Run attitude controllers */ if (_vcontrol_mode.flag_control_attitude_enabled) { if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) { diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index 4347cbfb28..ab4844ff02 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -137,6 +137,8 @@ private: float _battery_scale{1.0f}; + bool _flag_control_attitude_enabled_last{false}; + struct { float p_tc; float p_p;