From 6b779275bc2ba02ba2e6b90df7d4698e7b3d8174 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Jul 2013 20:42:13 +0200 Subject: [PATCH] Added quaternion renormalization --- src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp b/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp index d689d4eef5..32545e0e02 100644 --- a/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp +++ b/src/modules/fw_att_control_vector/ecl_fw_att_control_vector.cpp @@ -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;