fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 04:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_att_control_vector: bugs fixed

Browse Source
This commit is contained in:
Anton Babushkin
2013-12-21 21:49:37 +04:00
parent 38e5d2b0fb
commit ef5aa697c7
1 changed files with 3 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp
+3 -5
View File
@@ -373,9 +373,8 @@ MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[])
void void
MulticopterAttitudeControl::task_main() MulticopterAttitudeControl::task_main()
{ {
/* inform about start */ /* inform about start */
warnx("Initializing.."); warnx("started");
fflush(stdout); fflush(stdout);
/* /*
@@ -573,17 +572,16 @@ MulticopterAttitudeControl::task_main()
/* calculate angle error */ /* calculate angle error */
float e_R_z_sin = e_R.norm(); float e_R_z_sin = e_R.norm();
float e_R_z_cos = R_z * R_des_z; 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 */ /* calculate weight for yaw control */
float cos_z = cosf(R_z(2)); float yaw_w = R_des(2, 2) * R_des(2, 2);
float yaw_w = cos_z * cos_z;
/* calculate rotation matrix after roll/pitch only rotation */ /* calculate rotation matrix after roll/pitch only rotation */
math::Matrix R_rp(3, 3); math::Matrix R_rp(3, 3);
if (e_R_z_sin > 0.0f) { if (e_R_z_sin > 0.0f) {
/* get axis-angle representation */ /* 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; math::Vector3 e_R_z_axis = e_R / e_R_z_sin;
e_R = e_R_z_axis * e_R_z_angle; e_R = e_R_z_axis * e_R_z_angle;