From 69b1bfca6cc494c0a8b2cf94545d0f7fb9e44f62 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 21 Sep 2017 13:21:06 +0200 Subject: [PATCH] mc_att_control: fix yaw feedforwarding The feedforwarding was applied in the wrong frame: the term is given in global coordinates, but was directly applied to body coordinates. This patch adds the missing transformation from global to body frame. In addition, it moves the feedforwarding before the rate limitation, so that we cannot exceed the configured rates. --- .../mc_att_control/mc_att_control_main.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 264dc4bf88..0f8b32148f 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -932,6 +932,18 @@ MulticopterAttitudeControl::control_attitude(float dt) /* calculate angular rates setpoint */ _rates_sp = _params.att_p.emult(e_R); + + /* Feed forward the yaw setpoint rate. We need to transform the yaw from world into body frame. + * The following is a simplification of: + * R.transposed() * math::Vector<3>(0.f, 0.f, _v_att_sp.yaw_sp_move_rate * _params.yaw_ff) + */ + math::Vector<3> yaw_feedforward_rate(R(2, 0), R(2, 1), R(2, 2)); + yaw_feedforward_rate *= _v_att_sp.yaw_sp_move_rate * _params.yaw_ff; + + yaw_feedforward_rate(2) *= yaw_w; + _rates_sp += yaw_feedforward_rate; + + /* limit rates */ for (int i = 0; i < 3; i++) { if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) && @@ -943,9 +955,6 @@ MulticopterAttitudeControl::control_attitude(float dt) } } - /* feed forward yaw setpoint rate */ - _rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff; - /* weather-vane mode, dampen yaw rate */ if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) && _v_att_sp.disable_mc_yaw_control == true && !_v_control_mode.flag_control_manual_enabled) {