Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

AttitudeControl: Take advantage of Quaternion.canonical()

Browse Source 
Introduced by @kamilritz in
https://github.com/PX4/Matrix/pull/116/
This commit is contained in:
Matthias Grob
2020-03-12 11:28:03 +01:00
parent 9275937b44
commit d313b0417a
2 changed files with 4 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp
+2 -2
View File
@@ -77,7 +77,7 @@ matrix::Vector3f AttitudeControl::update(matrix::Quatf q, matrix::Quatf qd, cons
// mix full and reduced desired attitude // mix full and reduced desired attitude
Quatf q_mix = qd_red.inversed() * qd; 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 // catch numerical problems with the domain of acosf and asinf
q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f); q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f);
q_mix(3) = math::constrain(q_mix(3), -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) // 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 // 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 // calculate angular rates setpoint
matrix::Vector3f rate_setpoint = eq.emult(_proportional_gain); matrix::Vector3f rate_setpoint = eq.emult(_proportional_gain);
src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp
+2 -12
View File
@@ -63,6 +63,7 @@ public:
const Vector3f rate_setpoint_new = _attitude_control.update(_quat_state, _quat_goal, 0.f); const Vector3f rate_setpoint_new = _attitude_control.update(_quat_state, _quat_goal, 0.f);
// rotate the simulated state quaternion according to the rate setpoint // rotate the simulated state quaternion according to the rate setpoint
_quat_state = _quat_state * Quatf(AxisAnglef(rate_setpoint_new)); _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 // expect the error and hence also the output to get smaller with each iteration
if (rate_setpoint_new.norm() >= rate_setpoint.norm()) { if (rate_setpoint_new.norm() >= rate_setpoint.norm()) {
@@ -72,22 +73,11 @@ public:
rate_setpoint = rate_setpoint_new; 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 // it shouldn't have taken longer than an iteration timeout to converge
EXPECT_GT(i, 0); 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; AttitudeControl _attitude_control;
Quatf _quat_state; Quatf _quat_state;
Quatf _quat_goal; Quatf _quat_goal;