diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index ad0e6a5a8c..79dfabf5e3 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -58,6 +58,7 @@ #include #include #include +#include // NAN #include "sPort_data.h" #include "frsky_data.h" diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 3e53e32d47..acd30c8f3e 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -545,7 +545,7 @@ MK::task_main() for (unsigned int i = 0; i < _num_outputs; i++) { /* last resort: catch NaN, INF and out-of-band errors */ if (i < outputs.noutputs && - isfinite(outputs.output[i]) && + PX4_ISFINITE(outputs.output[i]) && outputs.output[i] >= -1.0f && outputs.output[i] <= 1.0f) { /* scale for PWM output 900 - 2100us */ diff --git a/src/drivers/navio_sysfs_pwm_out/navio_sysfs_pwm_out.cpp b/src/drivers/navio_sysfs_pwm_out/navio_sysfs_pwm_out.cpp index 8edaba8611..413edef304 100644 --- a/src/drivers/navio_sysfs_pwm_out/navio_sysfs_pwm_out.cpp +++ b/src/drivers/navio_sysfs_pwm_out/navio_sysfs_pwm_out.cpp @@ -37,6 +37,7 @@ #include #include #include +#include // NAN #include #include diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp index 185bda604d..73633952c2 100644 --- a/src/drivers/tap_esc/tap_esc.cpp +++ b/src/drivers/tap_esc/tap_esc.cpp @@ -38,6 +38,7 @@ #include #include #include +#include // NAN #include #include diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index ce27ccfcc3..e68e436a0c 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -44,7 +44,7 @@ #define MATRIX_HPP #include -#include +#include #include "matrix/math.hpp" #include diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp index fb929c9906..75a3fde4c9 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp @@ -39,11 +39,7 @@ #include #include "LowPassFilter2p.hpp" -#include "math.h" - -#ifndef M_PI_F -#define M_PI_F 3.14159f -#endif +#include namespace math { diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 55c7c15781..2240c1a7bb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -44,6 +44,8 @@ * @author Sander Smeets */ +#include // NAN + /* commander module headers */ #include "accelerometer_calibration.h" #include "airspeed_calibration.h" diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 91a0643e19..be7e5749e3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -54,7 +54,6 @@ #include #include #include -#include /* isinf / isnan checks */ #ifdef __PX4_POSIX #include diff --git a/src/modules/position_estimator_inav/inertial_filter.cpp b/src/modules/position_estimator_inav/inertial_filter.cpp index c70cbeb0ea..4f045aeb6b 100644 --- a/src/modules/position_estimator_inav/inertial_filter.cpp +++ b/src/modules/position_estimator_inav/inertial_filter.cpp @@ -5,14 +5,14 @@ * Author: Anton Babushkin */ -#include - +#include "px4_defines.h" #include "inertial_filter.h" +#include void inertial_filter_predict(float dt, float x[2], float acc) { - if (isfinite(dt)) { - if (!isfinite(acc)) { + if (PX4_ISFINITE(dt)) { + if (!PX4_ISFINITE(acc)) { acc = 0.0f; } @@ -23,7 +23,7 @@ void inertial_filter_predict(float dt, float x[2], float acc) void inertial_filter_correct(float e, float dt, float x[2], int i, float w) { - if (isfinite(e) && isfinite(w) && isfinite(dt)) { + if (PX4_ISFINITE(e) && PX4_ISFINITE(w) && PX4_ISFINITE(dt)) { float ewdt = e * w * dt; x[i] += ewdt; diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 2033fe36b4..2c170af023 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -54,6 +54,7 @@ #include #include #include +#include #include diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 8300e1e99b..56081e8d38 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -40,7 +40,10 @@ #pragma once #include -#include + +/**************************************************************************** + * Defines for all platforms. + ****************************************************************************/ /* Get the name of the default value fiven the param name */ #define PX4_PARAM_DEFAULT_VALUE_NAME(_name) PARAM_##_name##_DEFAULT @@ -52,10 +55,21 @@ #define PX4_ERROR (-1) #define PX4_OK 0 +/* Wrapper for 2d matrices */ +#define PX4_ARRAY2D(_array, _ncols, _x, _y) (_array[_x * _ncols + _y]) + +/* 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(x) std::isfinite(x) + #if defined(__PX4_ROS) -/* - * Building for running within the ROS environment - */ +/**************************************************************************** + * Building for running within the ROS environment. + ****************************************************************************/ + #define noreturn_function #ifdef __cplusplus #include "ros/ros.h" @@ -67,12 +81,11 @@ /* Get value of parameter by name, which is equal to the handle for ros */ #define PX4_PARAM_GET_BYNAME(_name, _destpt) ros::param::get(_name, *_destpt) -#define PX4_ISFINITE(x) std::isfinite(x) - #elif defined(__PX4_NUTTX) || defined(__PX4_POSIX) -/* - * Building for NuttX or POSIX - */ +/**************************************************************************** + * Building for NuttX or POSIX. + ****************************************************************************/ + #include /* Main entry point */ #define PX4_MAIN_FUNCTION(_prefix) int _prefix##_task_main(int argc, char *argv[]) @@ -84,43 +97,56 @@ typedef param_t px4_param_t; /* Get value of parameter by name */ #define PX4_PARAM_GET_BYNAME(_name, _destpt) param_get(param_find(_name), _destpt) -#else +#else // defined(__PX4_NUTTX) || defined(__PX4_POSIX) +/****************************************************************************/ #error "No target OS defined" #endif -/* - * NuttX Specific defines - */ #if defined(__PX4_NUTTX) +/**************************************************************************** + * NuttX specific defines. + ****************************************************************************/ #define PX4_ROOTFSDIR - #define _PX4_IOC(x,y) _IOC(x,y) - #define px4_statfs_buf_f_bavail_t int -#define PX4_ISFINITE(x) isfinite(x) - // mode for open with O_CREAT #define PX4_O_MODE_777 0777 #define PX4_O_MODE_666 0666 #define PX4_O_MODE_600 0600 #ifndef PRIu64 -#define PRIu64 "llu" +# define PRIu64 "llu" #endif #ifndef PRId64 -#define PRId64 "lld" +# define PRId64 "lld" #endif -#if !defined(offsetof) +#ifndef offsetof # define offsetof(TYPE, MEMBER) __builtin_offsetof (TYPE, MEMBER) #endif -/* - * POSIX Specific defines - */ +// Workaround for broken NuttX math.h. +#ifdef __cplusplus +#include +#undef isfinite +template +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 + ****************************************************************************/ // Flag is meaningless on Linux #define O_BINARY 0 @@ -130,7 +156,6 @@ typedef param_t px4_param_t; #define PX4_O_MODE_666 (S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH | S_IWOTH ) #define PX4_O_MODE_600 (S_IRUSR | S_IWUSR) - // NuttX _IOC is equivalent to Linux _IO #define _PX4_IOC(x,y) _IO(x,y) @@ -142,107 +167,79 @@ typedef param_t px4_param_t; /* FIXME - Used to satisfy build */ #define getreg32(a) (*(volatile uint32_t *)(a)) -#ifdef __PX4_QURT -#define PX4_TICKS_PER_SEC 1000L -#else -__BEGIN_DECLS -extern long PX4_TICKS_PER_SEC; -__END_DECLS -#endif - #define USEC_PER_TICK (1000000UL/PX4_TICKS_PER_SEC) #define USEC2TICK(x) (((x)+(USEC_PER_TICK/2))/USEC_PER_TICK) #define px4_statfs_buf_f_bavail_t unsigned long -#if defined(__PX4_QURT) -#define PX4_ROOTFSDIR -#elif defined(__PX4_POSIX_EAGLE) -#define PX4_ROOTFSDIR "/home/linaro" -#elif defined(__PX4_POSIX_BEBOP) -#define PX4_ROOTFSDIR "/home/root" -#else -#define PX4_ROOTFSDIR "rootfs" -#endif +#ifdef __PX4_QURT -#endif +// QURT specific +# include "dspal_math.h" +# define PX4_ROOTFSDIR +# define PX4_TICKS_PER_SEC 1000L +# define SIOCDEVPRIVATE 999999 +// HEXAGON defines isfinite() erroneously as a macro even for C++. +# undef PX4_ISFINITE +# define PX4_ISFINITE(x) isfinite(x) + +#else // __PX4_QURT + +// All POSIX except QURT. + +__BEGIN_DECLS +extern long PX4_TICKS_PER_SEC; +__END_DECLS + +# if defined(__PX4_POSIX_EAGLE) +# define PX4_ROOTFSDIR "/home/linaro" +# elif defined(__PX4_POSIX_BEBOP) +# define PX4_ROOTFSDIR "/home/root" +# else +# define PX4_ROOTFSDIR "rootfs" +# endif + +#endif // __PX4_QURT +#endif // __PX4_POSIX -/* - * Defines for ROS and Linux - */ #if defined(__PX4_ROS) || defined(__PX4_POSIX) +/**************************************************************************** + * Defines for POSIX and ROS + ****************************************************************************/ + #define OK 0 #define ERROR -1 - #define MAX_RAND 32767 -#if defined(__PX4_QURT) -#include "dspal_math.h" -__BEGIN_DECLS -#include -__END_DECLS -#else -#include -#endif +/* Math macro's for float literals. Do not use M_PI et al as they aren't + * defined (neither C nor the C++ standard define math constants) */ +#define M_E_F 2.71828183f +#define M_LOG2E_F 1.44269504f +#define M_LOG10E_F 0.43429448f +#define M_LN2_F 0.69314718f +#define M_LN10_F 2.30258509f +#define M_PI_F 3.14159265f +#define M_TWOPI_F 6.28318531f +#define M_PI_2_F 1.57079632f +#define M_PI_4_F 0.78539816f +#define M_3PI_4_F 2.35619449f +#define M_SQRTPI_F 1.77245385f +#define M_1_PI_F 0.31830989f +#define M_2_PI_F 0.63661977f +#define M_2_SQRTPI_F 1.12837917f +#define M_DEG_TO_RAD_F 0.0174532925f +#define M_RAD_TO_DEG_F 57.2957795f +#define M_SQRT2_F 1.41421356f +#define M_SQRT1_2_F 0.70710678f +#define M_LN2LO_F 1.90821484E-10f +#define M_LN2HI_F 0.69314718f +#define M_SQRT3_F 1.73205081f +#define M_IVLN10_F 0.43429448f // 1 / log(10) +#define M_LOG2_E_F 0.69314718f +#define M_INVLN2_F 1.44269504f // 1 / log(2) -/* Float defines of the standard double length constants */ -#define M_E_F (float)M_E -#define M_LOG2E_F (float)M_LOG2E -#define M_LOG10E_F (float)M_LOG10E -#define M_LN2_F (float)M_LN2 -#define M_LN10_F (float)M_LN10 -#define M_PI_F (float)M_PI -#define M_TWOPI_F (M_PI_F * 2.0f) -#define M_PI_2_F (float)M_PI_2 -#define M_PI_4_F (float)M_PI_4 -#define M_3PI_4_F (float)2.3561944901923448370E0f -#define M_SQRTPI_F (float)1.77245385090551602792981f -#define M_1_PI_F (float)M_1_PI -#define M_2_PI_F (float)M_2_PI -#define M_2_SQRTPI_F 1.12837916709551257390f -#define M_DEG_TO_RAD_F 0.01745329251994f -#define M_RAD_TO_DEG_F 57.2957795130823f -#define M_SQRT2_F (float)M_SQRT2 -#define M_SQRT1_2_F (float)M_SQRT1_2 -#define M_LN2LO_F 1.9082149292705877000E-10f -#define M_LN2HI_F 6.9314718036912381649E-1f -#define M_SQRT3_F 1.73205080756887719000f -#define M_IVLN10_F 0.43429448190325182765f /* 1 / log(10) */ -#define M_LOG2_E_F _M_LN2_F -#define M_INVLN2_F 1.4426950408889633870E0f/* 1 / log(2) */ -#define M_DEG_TO_RAD 0.01745329251994 -#define M_RAD_TO_DEG 57.2957795130823 +#define M_DEG_TO_RAD 0.017453292519943295 +#define M_RAD_TO_DEG 57.295779513082323 -#ifndef __PX4_QURT - -#if defined(__cplusplus) -#include -#define PX4_ISFINITE(x) std::isfinite(x) -#else -#define PX4_ISFINITE(x) isfinite(x) -#endif -#endif - -#endif - -#if defined(__PX4_QURT) - -#define DEFAULT_PARAM_FILE "/fs/eeprom/parameters" - -#define SIOCDEVPRIVATE 999999 - -// Missing math.h defines -#define PX4_ISFINITE(x) __builtin_isfinite(x) - -#endif - -/* - *Defines for all platforms - */ - -/* wrapper for 2d matrices */ -#define PX4_ARRAY2D(_array, _ncols, _x, _y) (_array[_x * _ncols + _y]) - -/* wrapper for rotation matrices stored in arrays */ -#define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y) +#endif // defined(__PX4_ROS) || defined(__PX4_POSIX)