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

AttitudeControlTest: fix adaptAntipodal() function

Browse Source
This commit is contained in:
Matthias Grob 2019-10-10 18:45:58 +02:00
parent 83dfa227a7
commit bb91db2057
1 changed files with 5 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp
View File

@ -72,26 +72,20 @@ public:
rate_setpoint = rate_setpoint_new;
}
// we need to have reached the goal attitude
if (!(antipodal(_quat_state) == (antipodal(_quat_goal)))) {
antipodal(_quat_state).print();
antipodal(_quat_goal).print();
}
EXPECT_TRUE(antipodal(_quat_state) == antipodal(_quat_goal));
EXPECT_EQ(adaptAntipodal(_quat_state), adaptAntipodal(_quat_goal));
// it shouldn't have taken longer than an iteration timeout to converge
EXPECT_GT(i, 0);
}
Quatf antipodal(const Quatf q)
Quatf adaptAntipodal(const Quatf q)
{
for (int i = 0; i < 1; i++) {
for (int i = 0; i < 4; i++) {
if (fabs(q(i)) > FLT_EPSILON) {
return q * math::signNoZero(q(i));
return q * math::sign(q(i));
}
}
return Quatf(NAN, NAN, NAN, NAN);
return q;
}
AttitudeControl _attitude_control;