diff options
-rw-r--r-- | src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp | 45 |
1 files changed, 42 insertions, 3 deletions
diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp index 834189a54..93806165d 100644 --- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp +++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp @@ -539,9 +539,48 @@ MulticopterAttitudeControl::task_main() /* current body angular rates */ math::Vector3 rates(_att.rollspeed, _att.pitchspeed, _att.yawspeed); - /* orientation error between R and R_des */ - math::Matrix e_R_m = (R_des.transpose() * R - R.transpose() * R_des).transpose() * 0.5f; - math::Vector3 e_R(-e_R_m(1, 2), e_R_m(0, 2), -e_R_m(0, 1)); + /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ + /* angle error between current and desired thrust vectors (Z axes) */ + math::Vector3 R_z(R(0, 2), R(1, 2), R(2, 2)); + math::Vector3 R_z_des(R_des(0, 2), R_des(1, 2), R_des(2, 2)); + math::Vector3 e_R = R_z.cross(R_z_des); + + /* convert angle error from NED to bady frame */ + e_R = R.transpose() * e_R; + + /* e_R has zero Z component, set it according to yaw */ + /* calculate rotation matrix after roll/pitch only rotation */ + math::Matrix R_rp(3, 3); + float e_R_angle = e_R.norm(); + if (e_R_angle > 0.0f) { + /* get axis-angle representation */ + math::Vector3 e_R_axis = e_R / e_R_angle; + /* cross product matrix for e_R_axis */ + math::Matrix e_R_cp(3, 3); + e_R_cp.setAll(0.0f); + e_R_cp(0, 1) = -e_R_axis(2); + e_R_cp(0, 2) = e_R_axis(1); + e_R_cp(1, 0) = e_R_axis(2); + e_R_cp(1, 2) = -e_R_axis(0); + e_R_cp(2, 0) = -e_R_axis(1); + e_R_cp(2, 1) = e_R_axis(0); + /* rotation matrix for roll/pitch only rotation */ + math::Matrix I(3, 3); + I.setAll(0.0f); + I(0, 0) = 1.0f; + I(1, 1) = 1.0f; + I(2, 2) = 1.0f; + R_rp = R * (I + e_R_cp * sinf(e_R_angle) + e_R_cp * e_R_cp * (1.0f - cosf(e_R_angle))); + } else { + /* zero roll/pitch rotation */ + R_rp = R; + } + + /* R_rp and R_des has the same Z axis, calculate yaw error */ + math::Vector3 R_des_z(R_des(0, 2), R_des(1, 2), R_des(2, 2)); + math::Vector3 R_des_x(R_des(0, 0), R_des(1, 0), R_des(2, 0)); + math::Vector3 R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); + e_R(2) = R_rp_x.cross(R_des_x) * R_des_z; /* angular rates setpoint*/ math::Vector3 rates_sp = _K * e_R; |