From ed0b6db2deec8be50afc7087dbd0a7d0990cb404 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 15 Mar 2018 17:45:32 +0100 Subject: [PATCH] mc_att_control: clarify comment about yaw feed forward --- src/modules/mc_att_control/mc_att_control_main.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 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 ed1999caf3..7d1d4acb71 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -878,10 +878,9 @@ MulticopterAttitudeControl::control_attitude(float dt) eq = eq.emult(attitude_gain); _rates_sp = math::Vector<3>(eq.data()); - /* 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() * Vector3f(0.f, 0.f, _v_att_sp.yaw_sp_move_rate * _params.yaw_ff) - */ + /* Feed forward the yaw setpoint rate. We need to apply the yaw rate in the body frame. + * We infer the body z axis by taking the last column of R.transposed (== q.inversed) + * because it's the rotation axis for body yaw and multiply it by the rate and gain. */ Vector3f yaw_feedforward_rate = q.inversed().dcm_z(); yaw_feedforward_rate *= _v_att_sp.yaw_sp_move_rate * _params.yaw_ff;