From baf89f4398ae9bc8bfdf6c0761aaba0667ba9db5 Mon Sep 17 00:00:00 2001 From: Carlo Wood Date: Mon, 24 Oct 2016 18:05:04 +0200 Subject: [PATCH] Clean up of px4_defines.h (remove math.h) This patch reorders px4_defines.h to make it more readable (I think) but more importantly, cleans up the #include / and [std::]isfinite stuff. My main goal was to completely get rid of including math.h/cmath, because that doesn't really belong in a header that is supposed to define macro's and is included in almost every source file (if not all). I'm not sure what it did before ;) (pun intended), but now it does the following: PX4_ISFINITE is only used in C++ code (that was already the case, but hereby is official; for C code just use 'isfinite()') and is defined to be std::isfinite, except on __PX4_QURT because that uses the HEXAGON toolset which (erroneously) defines isfinite as macro. I would have liked to remove PX4_ISFINITE completely from the code and just use std::isfinite whereever that is needed, but that would have required changing the libecl submodule, and at the moment I'm getting tired of changing submodules... so maybe something for the future. Also, all includes of or have been removed except for __PX4_NUTTX. Like the HEXAGON toolset NuttX currently defines isfinite as macro for C++. So, we could have solved this in the same was as __P4_QURT; but since we can fix NuttX ourselves I chose to add a kludge to px4_defines.h instead that fixes this problem, until the time that NuttX can be fixed (again postponing changing a submodule). The kludge still demands including , thus. After removal of the math header file, it needed to be included in source files that actually need it, of course. Finally, I had a look at the math macro's (like M_PI, M_PI_F, M_DEG_TO_RAD etc). These are sometimes (erroneously) defined in certain math.h header files (like both, hexagon and nuttx). This is incorrect: neither the C nor the C++ standard defines math constants (neither as macro nor otherwise). The "problem" here was that px4_defines.h defined some of the M_*_F float constants in terms of the M_* double constant, which are sometimes not defined either thus. So, I cleaned this up by defining the M_*_F math constants as float literals in px4_defines.h, except when they are defined in math.h for that platform. This means that math.h has to be always included when using those constants, but well; not much difference there as those files usually also need/use the macro NAN (which *is* a standard macro defined by math.h). Finally finally, DEFAULT_PARAM_FILE was removed as it isn't used anymore. All in all I think the resulting px4_defines.h is nice, giving me much less the feeling of a nearly unmaintainable and over time slowly growing collection of kludges and hacks. --- src/drivers/frsky_telemetry/frsky_telemetry.c | 1 + src/drivers/mkblctrl/mkblctrl.cpp | 2 +- .../navio_sysfs_pwm_out.cpp | 1 + src/drivers/tap_esc/tap_esc.cpp | 1 + src/lib/mathlib/math/Matrix.hpp | 2 +- .../mathlib/math/filter/LowPassFilter2p.cpp | 6 +- src/modules/commander/commander.cpp | 2 + src/modules/mavlink/mavlink_main.cpp | 1 - .../inertial_filter.cpp | 10 +- src/modules/systemlib/param/param.c | 1 + src/platforms/px4_defines.h | 223 +++++++++--------- 11 files changed, 124 insertions(+), 126 deletions(-) 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)