diff --git a/EKF/geo.cpp b/EKF/geo.cpp
index bb33d9ebf5..a1fa93ab1b 100644
--- a/EKF/geo.cpp
+++ b/EKF/geo.cpp
@@ -50,6 +50,15 @@
 #include <string.h>
 #include <float.h>
 
+#ifndef __PX4_QURT
+#if defined(__cplusplus) && !defined(__PX4_NUTTX)
+#include <cmath>
+#define ISFINITE(x) std::isfinite(x)
+#else
+#define ISFINITE(x) isfinite(x)
+#endif
+#endif
+
 /****************************************************************************
  *
  *   Copyright (c) 2014 MAV GEO Library (MAVGEO). All rights reserved.
@@ -709,7 +718,7 @@ float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
 float _wrap_pi(float bearing)
 {
 	/* value is inf or NaN */
-	if (!isfinite(bearing)) {
+	if (!ISFINITE(bearing)) {
 		return bearing;
 	}
 
@@ -739,7 +748,7 @@ float _wrap_pi(float bearing)
 float _wrap_2pi(float bearing)
 {
 	/* value is inf or NaN */
-	if (!isfinite(bearing)) {
+	if (!ISFINITE(bearing)) {
 		return bearing;
 	}
 
@@ -769,7 +778,7 @@ float _wrap_2pi(float bearing)
 float _wrap_180(float bearing)
 {
 	/* value is inf or NaN */
-	if (!isfinite(bearing)) {
+	if (!ISFINITE(bearing)) {
 		return bearing;
 	}
 
@@ -799,7 +808,7 @@ float _wrap_180(float bearing)
 float _wrap_360(float bearing)
 {
 	/* value is inf or NaN */
-	if (!isfinite(bearing)) {
+	if (!ISFINITE(bearing)) {
 		return bearing;
 	}