fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
matrix/helper_functions.hpp
View File

@ -31,7 +31,7 @@ bool is_finite(Type x) {
template<typename Type>
Type wrap(Type x, Type low, Type high) {
// already in range
if (low < x && x < high) {
if (low <= x && x < high) {
return x;
}

2
test/helper.cpp
View File

@ -14,6 +14,7 @@ int main()
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(-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)));
// wrap pi
@ -30,6 +31,7 @@ int main()
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(-201.0f) - (-201.0f + 32 * float(M_TWOPI))) < 10e-5);
TEST(fabs(wrap_2pi(0.0f)) < FLT_EPSILON);
TEST(!is_finite(wrap_2pi(NAN)));
Vector3f a(1, 2, 3);