From 184aa2861a8672d0ed828947c8a6dba474131796 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sun, 23 Dec 2018 16:11:05 -0500
Subject: [PATCH] PX4_ISFINITE use builtin everywhere

---
 .../kinetis/tone_alarm/ToneAlarmInterface.cpp |  1 +
 src/drivers/px4fmu/fmu.cpp                    |  1 +
 .../stm32/tone_alarm/ToneAlarmInterface.cpp   |  1 +
 src/modules/commander/PreflightCheck.cpp      |  2 ++
 src/modules/events/send_event.cpp             |  2 ++
 src/modules/land_detector/LandDetector.cpp    |  1 +
 src/modules/px4iofirmware/mixer.cpp           |  1 +
 .../uORB/uORB_tests/uORBTest_UnitTest.cpp     |  1 +
 src/modules/vmount/input_mavlink.cpp          |  1 +
 src/modules/vmount/input_rc.cpp               |  1 +
 src/modules/vmount/input_test.cpp             |  2 ++
 src/modules/vmount/output_mavlink.cpp         |  2 ++
 src/modules/vmount/vmount.cpp                 |  1 +
 .../df_bebop_bus_wrapper.cpp                  |  2 ++
 src/platforms/px4_defines.h                   | 31 +++----------------
 src/systemcmds/tests/test_float.cpp           |  2 ++
 src/systemcmds/tests/test_mathlib.cpp         | 20 ++++++++++++
 src/systemcmds/tests/test_microbench_math.cpp |  1 +
 src/systemcmds/tests/test_parameters.cpp      |  1 +
 src/systemcmds/tests/test_search_min.cpp      |  1 +
 src/systemcmds/tests/test_smooth_z.cpp        |  1 +
 21 files changed, 49 insertions(+), 27 deletions(-)

diff --git a/src/drivers/kinetis/tone_alarm/ToneAlarmInterface.cpp b/src/drivers/kinetis/tone_alarm/ToneAlarmInterface.cpp
index a625a5c2b9..e7f36bd2ab 100644
--- a/src/drivers/kinetis/tone_alarm/ToneAlarmInterface.cpp
+++ b/src/drivers/kinetis/tone_alarm/ToneAlarmInterface.cpp
@@ -43,6 +43,7 @@
 #include <px4_defines.h>
 #include <systemlib/px4_macros.h>
 
+#include <cmath>
 
 #define CAT3_(A, B, C)    A##B##C
 #define CAT3(A, B, C)     CAT3_(A, B, C)
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index fd03765d00..c24a07371a 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -38,6 +38,7 @@
  */
 
 #include <float.h>
+#include <math.h>
 
 #include <board_config.h>
 #include <drivers/device/device.h>
diff --git a/src/drivers/stm32/tone_alarm/ToneAlarmInterface.cpp b/src/drivers/stm32/tone_alarm/ToneAlarmInterface.cpp
index ddec21b4fd..b59b97360b 100644
--- a/src/drivers/stm32/tone_alarm/ToneAlarmInterface.cpp
+++ b/src/drivers/stm32/tone_alarm/ToneAlarmInterface.cpp
@@ -38,6 +38,7 @@
 #include <drivers/device/device.h>
 #include <lib/drivers/tone_alarm/ToneAlarmInterface.h>
 #include <px4_defines.h>
+#include <cmath>
 
 /* Check that tone alarm and HRT timers are different */
 #if defined(TONE_ALARM_TIMER) && defined(HRT_TIMER)
diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp
index c82e16461b..0584517006 100644
--- a/src/modules/commander/PreflightCheck.cpp
+++ b/src/modules/commander/PreflightCheck.cpp
@@ -44,6 +44,8 @@
 #include "health_flag_helper.h"
 #include "rc_check.h"
 
+#include <math.h>
+
 #include <parameters/param.h>
 #include <systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
diff --git a/src/modules/events/send_event.cpp b/src/modules/events/send_event.cpp
index 7b53ece223..2a80ff2c39 100644
--- a/src/modules/events/send_event.cpp
+++ b/src/modules/events/send_event.cpp
@@ -34,6 +34,8 @@
 #include "send_event.h"
 #include "temperature_calibration/temperature_calibration.h"
 
+#include <math.h>
+
 #include <px4_getopt.h>
 #include <px4_log.h>
 #include <drivers/drv_hrt.h>
diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp
index e3c8646212..c63bcf79ff 100644
--- a/src/modules/land_detector/LandDetector.cpp
+++ b/src/modules/land_detector/LandDetector.cpp
@@ -41,6 +41,7 @@
 #include "LandDetector.h"
 
 #include <float.h>
+#include <math.h>
 
 #include <px4_config.h>
 #include <px4_defines.h>
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index f853aad2cf..6f5bec9c22 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -46,6 +46,7 @@
 #include <stdbool.h>
 #include <float.h>
 #include <string.h>
+#include <math.h>
 
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_hrt.h>
diff --git a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
index 777578cbe4..8faf4811c3 100644
--- a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
+++ b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
@@ -38,6 +38,7 @@
 #include <stdio.h>
 #include <errno.h>
 #include <poll.h>
+#include <math.h>
 #include <lib/cdev/CDev.hpp>
 
 ORB_DEFINE(orb_test, struct orb_test, sizeof(orb_test), "ORB_TEST:int val;hrt_abstime time;");
diff --git a/src/modules/vmount/input_mavlink.cpp b/src/modules/vmount/input_mavlink.cpp
index a752ebf3bd..91b4f2dc7c 100644
--- a/src/modules/vmount/input_mavlink.cpp
+++ b/src/modules/vmount/input_mavlink.cpp
@@ -48,6 +48,7 @@
 #include <px4_defines.h>
 #include <px4_posix.h>
 #include <errno.h>
+#include <math.h>
 
 namespace vmount
 {
diff --git a/src/modules/vmount/input_rc.cpp b/src/modules/vmount/input_rc.cpp
index 1ab04a85c2..2d2494a54d 100644
--- a/src/modules/vmount/input_rc.cpp
+++ b/src/modules/vmount/input_rc.cpp
@@ -39,6 +39,7 @@
 
 #include "input_rc.h"
 
+#include <math.h>
 #include <errno.h>
 #include <px4_posix.h>
 #include <px4_defines.h>
diff --git a/src/modules/vmount/input_test.cpp b/src/modules/vmount/input_test.cpp
index 9f9d037cc1..e418bb7f75 100644
--- a/src/modules/vmount/input_test.cpp
+++ b/src/modules/vmount/input_test.cpp
@@ -39,6 +39,8 @@
 
 #include "input_test.h"
 
+#include <math.h>
+
 #include <px4_posix.h>
 
 
diff --git a/src/modules/vmount/output_mavlink.cpp b/src/modules/vmount/output_mavlink.cpp
index 9566fe1980..4e10867de8 100644
--- a/src/modules/vmount/output_mavlink.cpp
+++ b/src/modules/vmount/output_mavlink.cpp
@@ -40,6 +40,8 @@
 
 #include "output_mavlink.h"
 
+#include <math.h>
+
 #include <uORB/topics/vehicle_command.h>
 #include <px4_defines.h>
 
diff --git a/src/modules/vmount/vmount.cpp b/src/modules/vmount/vmount.cpp
index 0589f9be10..7d12e36212 100644
--- a/src/modules/vmount/vmount.cpp
+++ b/src/modules/vmount/vmount.cpp
@@ -43,6 +43,7 @@
  * Outputs to the mounts can be RC (PWM) output or mavlink.
  */
 
+#include <math.h>
 #include <stdlib.h>
 #include <stdio.h>
 #include <stdbool.h>
diff --git a/src/platforms/posix/drivers/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp b/src/platforms/posix/drivers/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp
index 868ce3c818..de1d77d910 100644
--- a/src/platforms/posix/drivers/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp
@@ -48,6 +48,8 @@
 
 #include <errno.h>
 #include <string.h>
+#include <math.h>
+
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_armed.h>
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 28a25350de..189e896a77 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -65,12 +65,11 @@
 /* Wrapper for rotation matrices stored in arrays */
 #define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y)
 
-/* Define a usable PX4_ISFINITE. Note that PX4_ISFINITE is ONLY used in C++ files,
- * therefore, by default, we want to use std::isfinite. */
+/* Define PX4_ISFINITE */
 #ifdef __cplusplus
-#include <cmath>
-#define PX4_ISFINITE(x) std::isfinite(x)
-#endif
+constexpr bool PX4_ISFINITE(float x) { return __builtin_isfinite(x); }
+constexpr bool PX4_ISFINITE(double x) { return __builtin_isfinite(x); }
+#endif /* __cplusplus */
 
 #if defined(__PX4_NUTTX) || defined(__PX4_POSIX)
 /****************************************************************************
@@ -117,22 +116,6 @@ typedef param_t px4_param_t;
 #  define offsetof(TYPE, MEMBER) __builtin_offsetof (TYPE, MEMBER)
 #endif
 
-// Workaround for broken NuttX math.h.
-#ifdef __cplusplus
-#include <cmath>
-#undef isfinite
-template<typename T>
-inline bool isfinite(T __y)
-{
-	int __cy = fpclassify(__y);
-	return __cy != FP_INFINITE && __cy != FP_NAN;
-}
-namespace std
-{
-using ::isfinite;
-}
-#endif // __cplusplus
-
 #elif defined(__PX4_POSIX)
 /****************************************************************************
  * POSIX Specific defines
@@ -165,12 +148,6 @@ using ::isfinite;
 #  define PX4_TICKS_PER_SEC 1000L
 #  define SIOCDEVPRIVATE 999999
 
-// HEXAGON's isfinite() is erroneously defined as a macro even for C++,
-// using std::isfinite (using ::isfinite) which is a function, but which
-// appears to be broken because of undefined symbols (ie, _Dtest (C linkage)).
-#  undef PX4_ISFINITE
-#  define PX4_ISFINITE(x) __builtin_isfinite(x)
-
 #else // __PX4_QURT
 
 // All POSIX except QURT.
diff --git a/src/systemcmds/tests/test_float.cpp b/src/systemcmds/tests/test_float.cpp
index d9049ccd83..b18f253804 100644
--- a/src/systemcmds/tests/test_float.cpp
+++ b/src/systemcmds/tests/test_float.cpp
@@ -39,7 +39,9 @@
 #include <unit_test.h>
 
 #include <px4_config.h>
+
 #include <float.h>
+#include <math.h>
 
 typedef union {
 	float f;
diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp
index 599c9f7f3e..3516edbd08 100644
--- a/src/systemcmds/tests/test_mathlib.cpp
+++ b/src/systemcmds/tests/test_mathlib.cpp
@@ -78,6 +78,7 @@ private:
 	bool testQuaternionfrom_dcm();
 	bool testQuaternionfrom_euler();
 	bool testQuaternionRotate();
+	bool testFinite();
 };
 
 #define TEST_OP(_title, _op) { unsigned int n = 30000; hrt_abstime t0, t1; t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { _op; }; t1 = hrt_absolute_time(); PX4_INFO(_title ": %.6fus", (double)(t1 - t0) / n); }
@@ -371,6 +372,24 @@ bool MathlibTest::testQuaternionRotate()
 	return true;
 }
 
+bool MathlibTest::testFinite()
+{
+	ut_assert("PX4_ISFINITE(0.0f)", PX4_ISFINITE(0.0f) == true);
+	ut_assert("PX4_ISFINITE(-0.0f)", PX4_ISFINITE(-0.0f) == true);
+	ut_assert("PX4_ISFINITE(1.0f)", PX4_ISFINITE(1.0f) == true);
+	ut_assert("PX4_ISFINITE(-1.0f)", PX4_ISFINITE(-1.0f) == true);
+
+	ut_assert("PX4_ISFINITE(NAN)", PX4_ISFINITE(NAN) == false);
+	ut_assert("PX4_ISFINITE(1/0)", PX4_ISFINITE(1.0f / 0.0f) == false);
+	ut_assert("PX4_ISFINITE(0/0)", PX4_ISFINITE(0.0f / 0.0f) == false);
+	ut_assert("PX4_ISFINITE(INFINITY)", PX4_ISFINITE(INFINITY) == false);
+	ut_assert("PX4_ISFINITE(NAN * INFINITY)", PX4_ISFINITE(NAN * INFINITY) == false);
+	ut_assert("PX4_ISFINITE(NAN * 1.0f)", PX4_ISFINITE(NAN * 1.0f) == false);
+	ut_assert("PX4_ISFINITE(INFINITY * 2.0f)", PX4_ISFINITE(INFINITY * 2.0f) == false);
+
+	return true;
+}
+
 bool MathlibTest::run_tests()
 {
 	ut_run_test(testVector2);
@@ -381,6 +400,7 @@ bool MathlibTest::run_tests()
 	ut_run_test(testQuaternionfrom_dcm);
 	ut_run_test(testQuaternionfrom_euler);
 	ut_run_test(testQuaternionRotate);
+	ut_run_test(testFinite);
 
 	return (_tests_failed == 0);
 }
diff --git a/src/systemcmds/tests/test_microbench_math.cpp b/src/systemcmds/tests/test_microbench_math.cpp
index 329c77c137..a208766352 100644
--- a/src/systemcmds/tests/test_microbench_math.cpp
+++ b/src/systemcmds/tests/test_microbench_math.cpp
@@ -41,6 +41,7 @@
 #include <time.h>
 #include <stdlib.h>
 #include <unistd.h>
+#include <math.h>
 
 #include <drivers/drv_hrt.h>
 #include <perf/perf_counter.h>
diff --git a/src/systemcmds/tests/test_parameters.cpp b/src/systemcmds/tests/test_parameters.cpp
index 3056ab0e2f..71efdae0fe 100644
--- a/src/systemcmds/tests/test_parameters.cpp
+++ b/src/systemcmds/tests/test_parameters.cpp
@@ -43,6 +43,7 @@
 #include <errno.h>
 #include <fcntl.h>
 #include <unistd.h>
+#include <math.h>
 
 class ParameterTest : public UnitTest
 {
diff --git a/src/systemcmds/tests/test_search_min.cpp b/src/systemcmds/tests/test_search_min.cpp
index a9642e40ad..da653b5154 100644
--- a/src/systemcmds/tests/test_search_min.cpp
+++ b/src/systemcmds/tests/test_search_min.cpp
@@ -38,6 +38,7 @@
 
 #include <unit_test.h>
 #include <float.h>
+#include <math.h>
 
 #include "../../lib/mathlib/math/SearchMin.hpp"
 
diff --git a/src/systemcmds/tests/test_smooth_z.cpp b/src/systemcmds/tests/test_smooth_z.cpp
index 7a6d2de868..8eba151fdd 100644
--- a/src/systemcmds/tests/test_smooth_z.cpp
+++ b/src/systemcmds/tests/test_smooth_z.cpp
@@ -39,6 +39,7 @@
 #include <unit_test.h>
 #include <lib/FlightTasks/tasks/Utility/ManualSmoothingZ.hpp>
 #include <float.h>
+#include <math.h>
 
 class SmoothZTest : public UnitTest
 {
-- 
GitLab