From 710c52980e0e7f3ea7a8e95fbceef27fca2ec736 Mon Sep 17 00:00:00 2001 From: Sebastian Verling Date: Thu, 23 May 2019 14:17:40 +0200 Subject: [PATCH] fixed calculation of magentic table values by using floor instead of int casting --- geo_lookup/geo_mag_declination.cpp | 5 +++-- mathlib/mathlib.cpp | 10 ++++++++++ mathlib/mathlib.h | 1 + 3 files changed, 14 insertions(+), 2 deletions(-) diff --git a/geo_lookup/geo_mag_declination.cpp b/geo_lookup/geo_mag_declination.cpp index 6055809267..8f329f6c12 100644 --- a/geo_lookup/geo_mag_declination.cpp +++ b/geo_lookup/geo_mag_declination.cpp @@ -49,6 +49,7 @@ #include using math::constrain; +using math::floorf; /** set this always to the sampling in degrees for the table below */ static constexpr float SAMPLING_RES = 10.0f; @@ -139,8 +140,8 @@ get_table_data(float lat, float lon, const int8_t table[13][37]) } /* round down to nearest sampling resolution */ - float min_lat = int(lat / SAMPLING_RES) * SAMPLING_RES; - float min_lon = int(lon / SAMPLING_RES) * SAMPLING_RES; + float min_lat = floorf(lat / SAMPLING_RES) * SAMPLING_RES; + float min_lon = floorf(lon / SAMPLING_RES) * SAMPLING_RES; /* find index of nearest low sampling point */ unsigned min_lat_index = get_lookup_table_index(&min_lat, SAMPLING_MIN_LAT, SAMPLING_MAX_LAT); diff --git a/mathlib/mathlib.cpp b/mathlib/mathlib.cpp index 69aee15ffc..fe2bc716df 100644 --- a/mathlib/mathlib.cpp +++ b/mathlib/mathlib.cpp @@ -70,6 +70,16 @@ float degrees(float radians) return (radians * 180.0f) / M_PI_F; } +int floorf(float input) +{ + int res = int(input); + if (res > input) { + res--; + } + + return res; +} + } // namespace math #endif /* ECL_STANDALONE */ diff --git a/mathlib/mathlib.h b/mathlib/mathlib.h index 04a1803257..e22a8ee508 100644 --- a/mathlib/mathlib.h +++ b/mathlib/mathlib.h @@ -60,6 +60,7 @@ float max(float val1, float val2); float constrain(float val, float min, float max); float radians(float degrees); float degrees(float radians); +int floorf(float input); } #else