From cd080289c6a41d54036e16d9e68718010beced43 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 12 Aug 2022 14:17:43 +0200 Subject: [PATCH] FW attitude controller: constrain rates correctly Signed-off-by: Silvan Fuhrer --- src/modules/fw_att_control/ecl_controller.cpp | 5 ----- src/modules/fw_att_control/ecl_controller.h | 1 - src/modules/fw_att_control/ecl_pitch_controller.cpp | 5 +++-- src/modules/fw_att_control/ecl_pitch_controller.h | 5 ----- src/modules/fw_att_control/ecl_roll_controller.cpp | 4 +++- src/modules/fw_att_control/ecl_yaw_controller.cpp | 5 +++-- 6 files changed, 9 insertions(+), 16 deletions(-) diff --git a/src/modules/fw_att_control/ecl_controller.cpp b/src/modules/fw_att_control/ecl_controller.cpp index 96040eaeda..9fec882ea4 100644 --- a/src/modules/fw_att_control/ecl_controller.cpp +++ b/src/modules/fw_att_control/ecl_controller.cpp @@ -103,11 +103,6 @@ void ECL_Controller::set_max_rate(float max_rate) _max_rate = max_rate; } -void ECL_Controller::set_bodyrate_setpoint(float rate) -{ - _body_rate_setpoint = math::constrain(rate, -_max_rate, _max_rate); -} - float ECL_Controller::get_euler_rate_setpoint() { return _euler_rate_setpoint; diff --git a/src/modules/fw_att_control/ecl_controller.h b/src/modules/fw_att_control/ecl_controller.h index 8eda0211ec..27cea46a74 100644 --- a/src/modules/fw_att_control/ecl_controller.h +++ b/src/modules/fw_att_control/ecl_controller.h @@ -87,7 +87,6 @@ public: void set_k_ff(float k_ff); void set_integrator_max(float max); void set_max_rate(float max_rate); - void set_bodyrate_setpoint(float rate); /* Getters */ float get_euler_rate_setpoint(); diff --git a/src/modules/fw_att_control/ecl_pitch_controller.cpp b/src/modules/fw_att_control/ecl_pitch_controller.cpp index 9a9055c963..34e07a8153 100644 --- a/src/modules/fw_att_control/ecl_pitch_controller.cpp +++ b/src/modules/fw_att_control/ecl_pitch_controller.cpp @@ -62,8 +62,9 @@ float ECL_PitchController::control_attitude(const float dt, const ECL_ControlDat _euler_rate_setpoint = pitch_error / _tc; /* Transform setpoint to body angular rates (jacobian) */ - _body_rate_setpoint = cosf(ctl_data.roll) * _euler_rate_setpoint + - cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.euler_yaw_rate_setpoint; + const float pitch_body_rate_setpoint_raw = cosf(ctl_data.roll) * _euler_rate_setpoint + + cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.euler_yaw_rate_setpoint; + _body_rate_setpoint = math::constrain(pitch_body_rate_setpoint_raw, -_max_rate_neg, _max_rate); return _body_rate_setpoint; } diff --git a/src/modules/fw_att_control/ecl_pitch_controller.h b/src/modules/fw_att_control/ecl_pitch_controller.h index 0b6c8fbca8..f7c4c18c21 100644 --- a/src/modules/fw_att_control/ecl_pitch_controller.h +++ b/src/modules/fw_att_control/ecl_pitch_controller.h @@ -73,11 +73,6 @@ public: _max_rate_neg = max_rate_neg; } - void set_bodyrate_setpoint(float rate) - { - _body_rate_setpoint = math::constrain(rate, -_max_rate_neg, _max_rate); - } - protected: float _max_rate_neg{0.0f}; }; diff --git a/src/modules/fw_att_control/ecl_roll_controller.cpp b/src/modules/fw_att_control/ecl_roll_controller.cpp index e3ae8a628e..d283389df0 100644 --- a/src/modules/fw_att_control/ecl_roll_controller.cpp +++ b/src/modules/fw_att_control/ecl_roll_controller.cpp @@ -61,7 +61,9 @@ float ECL_RollController::control_attitude(const float dt, const ECL_ControlData _euler_rate_setpoint = roll_error / _tc; /* Transform setpoint to body angular rates (jacobian) */ - _body_rate_setpoint = _euler_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.euler_yaw_rate_setpoint; + const float roll_body_rate_setpoint_raw = _euler_rate_setpoint - sinf(ctl_data.pitch) * + ctl_data.euler_yaw_rate_setpoint; + _body_rate_setpoint = math::constrain(roll_body_rate_setpoint_raw, -_max_rate, _max_rate); return _body_rate_setpoint; } diff --git a/src/modules/fw_att_control/ecl_yaw_controller.cpp b/src/modules/fw_att_control/ecl_yaw_controller.cpp index ba0cd5a076..fdb0878d46 100644 --- a/src/modules/fw_att_control/ecl_yaw_controller.cpp +++ b/src/modules/fw_att_control/ecl_yaw_controller.cpp @@ -85,8 +85,9 @@ float ECL_YawController::control_attitude(const float dt, const ECL_ControlData ctl_data.airspeed_min ? ctl_data.airspeed_min : ctl_data.airspeed); /* Transform setpoint to body angular rates (jacobian) */ - _body_rate_setpoint = -sinf(ctl_data.roll) * ctl_data.euler_pitch_rate_setpoint + - cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _euler_rate_setpoint; + const float yaw_body_rate_setpoint_raw = -sinf(ctl_data.roll) * ctl_data.euler_pitch_rate_setpoint + + cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _euler_rate_setpoint; + _body_rate_setpoint = math::constrain(yaw_body_rate_setpoint_raw, -_max_rate, _max_rate); } if (!PX4_ISFINITE(_body_rate_setpoint)) {