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

Update examples

Browse Source
This commit is contained in:
Lorenz Meier
2016-10-18 20:46:08 +02:00
parent 873ed17b14
commit ac936a28dd
3 changed files with 10 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/examples/mc_att_control_multiplatform/mc_att_control_base.cpp
+4 -4
View File
@@ -147,11 +147,11 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
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 */
math::Quaternion q;
q.from_dcm(R.transposed() * R_sp);
math::Vector <3> e_R_d = q.imag();
math::Quaternion ql;
ql.from_dcm(R.transposed() * R_sp);
math::Vector <3> e_R_d = ql.imag();
e_R_d.normalize();
e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));
e_R_d *= 2.0f * atan2f(e_R_d.length(), ql(0));
/* use fusion of Z axis based rotation and direct rotation */
float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;