Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 12:38:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: fix loss of precision on float addition

Browse Source 
When using wrap_180_cd() we are adding a small float (180 * 100) to a
possibly big number. This may lose float precision as illustrated by the
unit test failing:

    OUT: ../../libraries/AP_Math/tests/test_math.cpp:195: Failure
    OUT: Value of: wrap_180_cd(-3600000000.f)
    OUT:   Actual: -80
    OUT: Expected: 0.f
    OUT: Which is: 0
This commit is contained in:
Lucas De Marchi 2016-05-02 13:23:12 -03:00
parent 76362caee0
commit 880f130670
2 changed files with 27 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_Math/AP_Math.cpp
View File

@ -52,13 +52,9 @@ float linear_interpolate(float low_output, float high_output,
template <class T> template <class T>
float wrap_180(const T angle, float unit_mod) float wrap_180(const T angle, float unit_mod)
{ {
const float ang_180 = 180.f * unit_mod; auto res = wrap_360(angle, unit_mod);
const float ang_360 = 360.f * unit_mod; if (res > 180.f * unit_mod) {
float res = fmod(static_cast<float>(angle) + ang_180, ang_360); res -= 360.f * unit_mod;
if (res < 0 || is_zero(res)) {
res += ang_180;
} else {
res -= ang_180;
} }
return res; return res;
} }
@ -109,11 +105,9 @@ template auto wrap_360_cd<double>(const double angle) -> decltype(wrap_360(angle
template <class T> template <class T>
float wrap_PI(const T radian) float wrap_PI(const T radian)
{ {
float res = fmod(static_cast<float>(radian) + M_PI, M_2PI); auto res = wrap_2PI(radian);
if (res < 0 || is_zero(res)) { if (res > M_PI) {
res += M_PI; res -= M_2PI;
} else {
res -= M_PI;
} }
return res; return res;
} }

21
libraries/AP_Math/tests/test_math.cpp
View File

@ -240,4 +240,25 @@ TEST(MathWrapTest, Angle360)
EXPECT_EQ(0.f, wrap_360_cd(-72000.f)); EXPECT_EQ(0.f, wrap_360_cd(-72000.f));
} }
TEST(MathWrapTest, AnglePI)
{
const float accuracy = 1.0e-5;
EXPECT_NEAR(M_PI, wrap_PI(M_PI), accuracy);
EXPECT_NEAR(0.f, wrap_PI(M_2PI), accuracy);
EXPECT_NEAR(0, wrap_PI(M_PI * 10), accuracy);
}
TEST(MathWrapTest, Angle2PI)
{
const float accuracy = 1.0e-5;
EXPECT_NEAR(M_PI, wrap_2PI(M_PI), accuracy);
EXPECT_NEAR(0.f, wrap_2PI(M_2PI), accuracy);
EXPECT_NEAR(0.f, wrap_2PI(M_PI * 10), accuracy);
EXPECT_NEAR(0.f, wrap_2PI(0.f), accuracy);
EXPECT_NEAR(M_PI, wrap_2PI(-M_PI), accuracy);
EXPECT_NEAR(0, wrap_2PI(-M_2PI), accuracy);
}
AP_GTEST_MAIN() AP_GTEST_MAIN()