diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2013-12-26 23:02:09 +0400 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2013-12-26 23:02:09 +0400 |
| commit | e103729de308c113d561942335a4bcf20c68a255 (patch) | |
| tree | 1711495185d7e03f384cd3e58e4ef01fb7afc876 /src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp | |
| parent | 26daae0b0af2f73171f2e741178474873724fdbe (diff) | |
| download | px4-firmware-e103729de308c113d561942335a4bcf20c68a255.tar.gz px4-firmware-e103729de308c113d561942335a4bcf20c68a255.tar.bz2 px4-firmware-e103729de308c113d561942335a4bcf20c68a255.zip | |
mc_att_control_vector: fixes, parameters added
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 | 5 |
1 files changed, 3 insertions, 2 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 30c073c29..ef6a46810 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 @@ -616,12 +616,13 @@ MulticopterAttitudeControl::task_main() math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; - if (e_R_z_cos < 0.0) { + if (e_R_z_cos < 0.0f) { /* for large thrust vector rotations use another rotation method: * calculate angle and axis for R -> R_sp rotation directly */ q.from_dcm(R.transposed() * R_sp); math::Vector<3> e_R_d = q.imag(); - float angle_d = 2.0f * atan2f(e_R_d.length(), q(0)); + e_R_d.normalize(); + e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); /* use fusion of Z axis based rotation and direct rotation */ float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; |