diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-12-21 16:21:16 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-12-21 16:21:16 +0400 |
commit | 05e9a30573f50dd271f10864f6e7ec37b6c94043 (patch) | |
tree | 3938710a2f8f8eab0dffd88e3431fb46678921cb /src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp | |
parent | 53192b5f4d11237c353faed72b326e67004fcef7 (diff) | |
download | px4-firmware-05e9a30573f50dd271f10864f6e7ec37b6c94043.tar.gz px4-firmware-05e9a30573f50dd271f10864f6e7ec37b6c94043.tar.bz2 px4-firmware-05e9a30573f50dd271f10864f6e7ec37b6c94043.zip |
multirotor_pos_control & mc_att_control_vector: singularities handling improved
Diffstat (limited to 'src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp')
-rw-r--r-- | src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp | 56 |
1 files changed, 35 insertions, 21 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 230fa15e4..fb09078fa 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 @@ -564,33 +564,39 @@ MulticopterAttitudeControl::task_main() math::Vector3 rates(_att.rollspeed, _att.pitchspeed, _att.yawspeed); /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ - /* sin(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.transpose() * R_z.cross(R_z_des); + math::Vector3 R_des_z(R_des(0, 2), R_des(1, 2), R_des(2, 2)); + + /* axis and sin(angle) of desired rotation */ + math::Vector3 e_R = R.transpose() * R_z.cross(R_des_z); /* calculate angle error */ - float e_R_sin = e_R.norm(); - float e_R_cos = R_z * R_z_des; - float angle = atan2f(e_R_sin, e_R_cos); + float e_R_z_sin = e_R.norm(); + float e_R_z_cos = R_z * R_des_z; + float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); + + /* calculate weight for yaw control */ + float cos_z = cosf(R_z(2)); + float yaw_w = cos_z * cos_z; - /* 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); - if (e_R_sin > 0.0f) { + if (e_R_z_sin > 0.0f) { /* get axis-angle representation */ - math::Vector3 e_R_axis = e_R / e_R_sin; + math::Vector3 e_R_z_axis = e_R / e_R_z_sin; + + e_R = e_R_z_axis * e_R_z_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); + e_R_cp(0, 1) = -e_R_z_axis(2); + e_R_cp(0, 2) = e_R_z_axis(1); + e_R_cp(1, 0) = e_R_z_axis(2); + e_R_cp(1, 2) = -e_R_z_axis(0); + e_R_cp(2, 0) = -e_R_z_axis(1); + e_R_cp(2, 1) = e_R_z_axis(0); /* rotation matrix for roll/pitch only rotation */ math::Matrix I(3, 3); @@ -598,7 +604,7 @@ MulticopterAttitudeControl::task_main() I(0, 0) = 1.0f; I(1, 1) = 1.0f; I(2, 2) = 1.0f; - R_rp = R * (I + e_R_cp * e_R_sin + e_R_cp * e_R_cp * (1.0f - e_R_cos)); + R_rp = R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); } else { /* zero roll/pitch rotation */ @@ -606,13 +612,21 @@ MulticopterAttitudeControl::task_main() } /* 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; - - /* don't try to control yaw when thrust vector has opposite direction to desired */ - e_R(2) *= (e_R_cos > 0.0f ? 1.0f : e_R_sin); + e_R(2) = atan2f(R_rp_x.cross(R_des_x) * R_des_z, R_rp_x * R_des_x) * yaw_w; + + if (e_R_z_cos < 0.0) { + /* for large thrust vector rotations use another rotation method: + * calculate angle and axis for R -> R_des rotation directly */ + math::Quaternion q(R.transpose() * R_des); + float angle_d = 2.0f * atan2f(sqrtf(q.getB() * q.getB() + q.getC() * q.getC() + q.getD() * q.getD()), q.getA()); + math::Vector3 e_R_d(q.getB(), q.getC(), q.getD()); + + /* use fusion of Z axis based rotation and direct rotation */ + float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; + e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; + } /* angular rates setpoint*/ math::Vector3 rates_sp = _K * e_R; |