diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2013-12-21 21:49:37 +0400 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2013-12-21 21:49:37 +0400 |
| commit | ef5aa697c73e095f21143a64eb3cd112c85b3a08 (patch) | |
| tree | b733d86ac12b74350355c1913ce0ab137f7eb35c /src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp | |
| parent | 38e5d2b0fbff18f53bb86dada8def5302c36a3d7 (diff) | |
| download | px4-firmware-ef5aa697c73e095f21143a64eb3cd112c85b3a08.tar.gz px4-firmware-ef5aa697c73e095f21143a64eb3cd112c85b3a08.tar.bz2 px4-firmware-ef5aa697c73e095f21143a64eb3cd112c85b3a08.zip | |
mc_att_control_vector: bugs fixed
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 | 8 |
1 files changed, 3 insertions, 5 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 fb09078fa..63aece1d9 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 @@ -373,9 +373,8 @@ MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) void MulticopterAttitudeControl::task_main() { - /* inform about start */ - warnx("Initializing.."); + warnx("started"); fflush(stdout); /* @@ -573,17 +572,16 @@ MulticopterAttitudeControl::task_main() /* calculate angle error */ 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; + float yaw_w = R_des(2, 2) * R_des(2, 2); /* calculate rotation matrix after roll/pitch only rotation */ math::Matrix R_rp(3, 3); if (e_R_z_sin > 0.0f) { /* get axis-angle representation */ + float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); math::Vector3 e_R_z_axis = e_R / e_R_z_sin; e_R = e_R_z_axis * e_R_z_angle; |