fixed calculation of magentic table values by using floor instead of int casting

geo_lookup/geo_mag_declination.cpp

@@ -49,6 +49,7 @@
 #include <stdint.h>
 
 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_lat = floorf(lat / SAMPLING_RES) * SAMPLING_RES;
-	float min_lon = int(lon / 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);

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 */

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