fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 03:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix bug in matrix wrap function (#83)

Browse Source
This commit is contained in:
Tanja Baumann
2019-09-05 11:21:41 +02:00
committed by Julian Kent
parent 4a4309327a
commit 26fd962cbe
2 changed files with 3 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/helper_functions.hpp
+1 -1
View File
@@ -31,7 +31,7 @@ bool is_finite(Type x) {
template<typename Type> template<typename Type>
Type wrap(Type x, Type low, Type high) { Type wrap(Type x, Type low, Type high) {
// already in range // already in range
if (low < x && x < high) { if (low <= x && x < high) {
return x; return x;
} }
test/helper.cpp
+2
View File
@@ -14,6 +14,7 @@ int main()
TEST(fabs(wrap(15.5, 3.0, 5.0) - 3.5) < FLT_EPSILON); TEST(fabs(wrap(15.5, 3.0, 5.0) - 3.5) < FLT_EPSILON);
TEST(fabs(wrap(-1.0, 30.0, 40.0) - 39.0) < FLT_EPSILON); TEST(fabs(wrap(-1.0, 30.0, 40.0) - 39.0) < FLT_EPSILON);
TEST(fabs(wrap(-8000.0, -555.0, 1.0) - (-216.0)) < FLT_EPSILON); TEST(fabs(wrap(-8000.0, -555.0, 1.0) - (-216.0)) < FLT_EPSILON);
TEST(fabs(wrap(0.0, 0.0, 360.0)) < FLT_EPSILON);
TEST(!is_finite(wrap(1000.,0.,.01))); TEST(!is_finite(wrap(1000.,0.,.01)));
// wrap pi // wrap pi
@@ -30,6 +31,7 @@ int main()
TEST(fabs(wrap_2pi(3.0) - (3.0)) < FLT_EPSILON); TEST(fabs(wrap_2pi(3.0) - (3.0)) < FLT_EPSILON);
TEST(fabs(wrap_2pi(200.0f) - (200.0f - 31 * float(M_TWOPI))) < 10e-5); TEST(fabs(wrap_2pi(200.0f) - (200.0f - 31 * float(M_TWOPI))) < 10e-5);
TEST(fabs(wrap_2pi(-201.0f) - (-201.0f + 32 * float(M_TWOPI))) < 10e-5); TEST(fabs(wrap_2pi(-201.0f) - (-201.0f + 32 * float(M_TWOPI))) < 10e-5);
TEST(fabs(wrap_2pi(0.0f)) < FLT_EPSILON);
TEST(!is_finite(wrap_2pi(NAN))); TEST(!is_finite(wrap_2pi(NAN)));
Vector3f a(1, 2, 3); Vector3f a(1, 2, 3);