From a9ca8c294c9d6f5901fe38f9b1920cdfde9bd451 Mon Sep 17 00:00:00 2001
From: waltjohnson <walt@inertialsense.com>
Date: Thu, 19 May 2016 10:34:16 -0600
Subject: [PATCH] Generalized isfinite() in similar form as done in ekf.cpp 
 https://github.com/PX4/ecl/blob/e3b9800cac459e7c279395c18191a872f465f848/EKF/ekf.cpp#L45-L52

---
 EKF/geo.cpp | 17 +++++++++++++----
 1 file changed, 13 insertions(+), 4 deletions(-)

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;
 	}