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

Added quaternion renormalization

Browse Source
This commit is contained in:
Lorenz Meier 2013-07-13 20:42:13 +02:00
parent cbcc124c71
commit 6b779275bc
1 changed files with 2 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp
View File

@ -131,6 +131,8 @@ void ECL_FWAttControlVector::control(float dt, float airspeed, float airspeed_sc
// XXX switch between operations based on difference,
// benchmark both options
math::Quaternion e_q = math::Quaternion(R_des) - math::Quaternion(R_bn);
// Renormalize
e_q = e_q / e_q.norm();
math::Matrix e_R = math::Dcm(e_q);
//small angles: math::Matrix e_R = (R_des.transpose() * R_bn - R_bn.transpose() * R_des) * 0.5f;