PX4_ISFINITE use builtin everywhere

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)

src/drivers/px4fmu/fmu.cpp

@@ -38,6 +38,7 @@
  */
 
 #include <float.h>
+#include <math.h>
 
 #include <board_config.h>
 #include <drivers/device/device.h>

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)

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>

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>

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>

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>

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;");

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
 {

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>

src/modules/vmount/input_test.cpp

@@ -39,6 +39,8 @@
 
 #include "input_test.h"
 
+#include <math.h>
+
 #include <px4_posix.h>
 
 

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>
 

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>

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>

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.

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;

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

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>

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
 {

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"
 

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
 {