dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

calculate large error rotation correclty in attitude controller

This commit is contained in:
Roman 2016-04-18 11:51:58 +02:00 committed by Lorenz Meier
parent 9daf6c336b
commit cf7145c8d9
1 changed files with 3 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -712,11 +712,9 @@ MulticopterAttitudeControl::control_attitude(float dt)
if (e_R_z_cos < 0.0f) { if (e_R_z_cos < 0.0f) {
/* for large thrust vector rotations use another rotation method: /* for large thrust vector rotations use another rotation method:
* calculate angle and axis for R -> R_sp rotation directly */ * calculate angle and axis for R -> R_sp rotation directly */
math::Quaternion q; math::Quaternion q_error;
q.from_dcm(R.transposed() * R_sp); q_error.from_dcm(R.transposed() * R_sp);
math::Vector<3> e_R_d = q.imag(); math::Vector<3> e_R_d = q_error(0) >= 0.0f ? q_error.imag() * 2.0f: -q_error.imag() * 2.0f;
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 */ /* use fusion of Z axis based rotation and direct rotation */
float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;