From 55da07d3c42ac48f16c7a587ab42dcc73ef507d2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 21 Sep 2017 13:34:33 +0200 Subject: [PATCH] mc_att_control: fix computation of yaw weight for attitude control Previously, the yaw weight was based on the tilt angle of the attitude setpoint (R_sp(2, 2) == cos(tilt angle)). This makes no sense, it means the weight is low for high tilt angles even if there is no roll-pitch error at all. This patch changes the weight computation to be based on the tilt angle error: the yaw weight is 1 if there is no roll-pitch error (independent from current tilt angle), and is reduced for higher tilt angle errors. The weight was added in 05e9a30573f50dd271f10, without any explanation or derivation of how and why the weight is chosen that way. However this patch works much better in practice. The yaw control is improved, though it can be a bit slow to converge if you do continuous & fast roll-pitch motions (which is expected). --- src/modules/mc_att_control/mc_att_control_main.cpp | 11 ++++++++--- 1 file changed, 8 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 06d709af62..4e4e9392d8 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -882,9 +882,6 @@ MulticopterAttitudeControl::control_attitude(float dt) float e_R_z_sin = e_R.length(); // == sin(tilt angle error) float e_R_z_cos = R_z * R_sp_z; // == cos(tilt angle error) == (R.transposed() * R_sp)(2, 2) - /* calculate weight for yaw control */ - float yaw_w = R_sp(2, 2) * R_sp(2, 2); - /* calculate rotation matrix after roll/pitch only rotation */ math::Matrix<3, 3> R_rp; @@ -916,6 +913,14 @@ MulticopterAttitudeControl::control_attitude(float dt) /* R_rp and R_sp have the same Z axis, calculate yaw error */ math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); + + /* calculate the weight for yaw control + * Make the weight depend on the tilt angle error: the higher the error of roll and/or pitch, the lower + * the weight that we use to control the yaw. This gives precedence to roll & pitch correction. + * The weight is 1 if there is no tilt error. + */ + float yaw_w = e_R_z_cos * e_R_z_cos; + /* calculate the angle between R_rp_x and R_sp_x (yaw angle error), and apply the yaw weight */ e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;