fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-05 00:21:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

AttitudeControlTest: fix adaptAntipodal() function

Browse Source
This commit is contained in:
Matthias Grob
2019-10-14 18:23:22 +02:00
parent 83dfa227a7
commit bb91db2057
1 changed files with 5 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/AttitudeControl/AttitudeControlTest.cpp
+5 -11
View File
@@ -72,26 +72,20 @@ public:
rate_setpoint = rate_setpoint_new; rate_setpoint = rate_setpoint_new;
} }
// we need to have reached the goal attitude EXPECT_EQ(adaptAntipodal(_quat_state), adaptAntipodal(_quat_goal));
if (!(antipodal(_quat_state) == (antipodal(_quat_goal)))) {
antipodal(_quat_state).print();
antipodal(_quat_goal).print();
}
EXPECT_TRUE(antipodal(_quat_state) == antipodal(_quat_goal));
// 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 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) { 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; AttitudeControl _attitude_control;