From d313b0417a10f93445a2d0a296390590daca0fd0 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 12 Mar 2020 11:28:03 +0100 Subject: [PATCH] AttitudeControl: Take advantage of Quaternion.canonical() Introduced by @kamilritz in https://github.com/PX4/Matrix/pull/116/ --- .../AttitudeControl/AttitudeControl.cpp | 4 ++-- .../AttitudeControl/AttitudeControlTest.cpp | 14 ++------------ 2 files changed, 4 insertions(+), 14 deletions(-) diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp index 7a348117f7..42a3aa9f38 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp @@ -77,7 +77,7 @@ matrix::Vector3f AttitudeControl::update(matrix::Quatf q, matrix::Quatf qd, cons // mix full and reduced desired attitude Quatf q_mix = qd_red.inversed() * qd; - q_mix *= math::signNoZero(q_mix(0)); + q_mix.canonicalize(); // catch numerical problems with the domain of acosf and asinf q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f); q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f); @@ -88,7 +88,7 @@ matrix::Vector3f AttitudeControl::update(matrix::Quatf q, matrix::Quatf qd, cons // using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle) // also taking care of the antipodal unit quaternion ambiguity - const Vector3f eq = 2.f * math::signNoZero(qe(0)) * qe.imag(); + const Vector3f eq = 2.f * qe.canonical().imag(); // calculate angular rates setpoint matrix::Vector3f rate_setpoint = eq.emult(_proportional_gain); diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp index c57ce9a8e2..b6a321c125 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp @@ -63,6 +63,7 @@ public: const Vector3f rate_setpoint_new = _attitude_control.update(_quat_state, _quat_goal, 0.f); // rotate the simulated state quaternion according to the rate setpoint _quat_state = _quat_state * Quatf(AxisAnglef(rate_setpoint_new)); + _quat_state = -_quat_state; // produce intermittent antipodal quaternion states to test against unwinding problem // expect the error and hence also the output to get smaller with each iteration if (rate_setpoint_new.norm() >= rate_setpoint.norm()) { @@ -72,22 +73,11 @@ public: rate_setpoint = rate_setpoint_new; } - EXPECT_EQ(adaptAntipodal(_quat_state), adaptAntipodal(_quat_goal)); + EXPECT_EQ(_quat_state.canonical(), _quat_goal.canonical()); // it shouldn't have taken longer than an iteration timeout to converge EXPECT_GT(i, 0); } - Quatf adaptAntipodal(const Quatf q) - { - for (int i = 0; i < 4; i++) { - if (fabs(q(i)) > FLT_EPSILON) { - return q * math::sign(q(i)); - } - } - - return q; - } - AttitudeControl _attitude_control; Quatf _quat_state; Quatf _quat_goal;