Merged in LP-621_fix_isnan_with_fast-math (pull request #543)

LP-621_fix_isnan_with_fast-math

Approved-by: Vladimir Zidar

flight/libraries/math/mathmisc.c

@@ -29,6 +29,7 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
+#include <pios_math.h>
 #include <mathmisc.h>
 
 void pseudo_windowed_variance_init(pw_variance_t *variance, int32_t window_size)

flight/libraries/math/mathmisc.h

@@ -107,7 +107,7 @@ static inline void vector_normalizef(float *vector, const uint8_t dim)
 {
     float length = vector_lengthf(vector, dim);
 
-    if (length <= 0.0f || isnan(length)) {
+    if (length <= 0.0f || !IS_REAL(length)) {
         return;
     }
     for (int t = 0; t < dim; t++) {

flight/modules/Attitude/attitude.c

@@ -538,7 +538,7 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
     accels[2] *= accel_scale.Z * invcount;
     temp *= invcount;
 
-    if (isnan(temperature)) {
+    if (!IS_REAL(temperature)) {
         temperature = temp;
     }
     temperature = temp_alpha * (temp - temperature) + temperature;
@@ -740,7 +740,7 @@ __attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accel
 
     // If quaternion has become inappropriately short or is nan reinit.
     // THIS SHOULD NEVER ACTUALLY HAPPEN
-    if ((fabsf(inv_qmag) > 1e3f) || isnan(inv_qmag)) {
+    if ((fabsf(inv_qmag) > 1e3f) || !IS_REAL(inv_qmag)) {
         q[0] = 1;
         q[1] = 0;
         q[2] = 0;

flight/modules/Sensors/sensors.c

@@ -515,7 +515,7 @@ static void handleBaro(float sample, float temperature)
 
     float altitude = 44330.0f * (1.0f - powf((sample) / PIOS_CONST_MKS_STD_ATMOSPHERE_F, (1.0f / 5.255f)));
 
-    if (!isnan(altitude)) {
+    if (IS_REAL(altitude)) {
         BaroSensorData data;
         data.Altitude    = altitude;
         data.Temperature = temperature;
@@ -527,7 +527,7 @@ static void handleBaro(float sample, float temperature)
 
 static void updateAccelTempBias(float temperature)
 {
-    if (isnan(accel_temperature)) {
+    if (!IS_REAL(accel_temperature)) {
         accel_temperature = temperature;
     }
     accel_temperature = temp_alpha_gyro_accel * (temperature - accel_temperature) + accel_temperature;
@@ -549,7 +549,7 @@ static void updateAccelTempBias(float temperature)
 
 static void updateGyroTempBias(float temperature)
 {
-    if (isnan(gyro_temperature)) {
+    if (!IS_REAL(gyro_temperature)) {
         gyro_temperature = temperature;
     }
 
@@ -570,7 +570,7 @@ static void updateGyroTempBias(float temperature)
 
 static void updateBaroTempBias(float temperature)
 {
-    if (isnan(baro_temperature)) {
+    if (!IS_REAL(baro_temperature)) {
         baro_temperature = temperature;
     }
 

flight/modules/StateEstimation/filtercf.c

@@ -529,7 +529,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
 
     // If quaternion has become inappropriately short or is nan reinit.
     // THIS SHOULD NEVER ACTUALLY HAPPEN
-    if ((fabsf(inv_qmag) > 1e3f) || isnan(inv_qmag)) {
+    if ((fabsf(inv_qmag) > 1e3f) || !IS_REAL(inv_qmag)) {
         this->first_run = 1;
         return FILTERRESULT_WARNING;
     }

flight/modules/StateEstimation/filterekf.c

@@ -481,7 +481,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
 // check for invalid variance values
 static inline bool invalid_var(float data)
 {
-    if (isnan(data) || isinf(data)) {
+    if (!IS_REAL(data)) {
         return true;
     }
     if (data < 1e-15f) { // var should not be close to zero. And not negative either.

flight/modules/StateEstimation/filtermag.c

@@ -282,9 +282,9 @@ void magOffsetEstimation(struct data *this, float mag[3])
     delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]);
     delta[2] = -rate * (Rz - B_e[2]);
 
-    if (!isnan(delta[0]) && !isinf(delta[0]) &&
+    if (IS_REAL(delta[0]) &&
-        !isnan(delta[1]) && !isinf(delta[1]) &&
+        IS_REAL(delta[1]) &&
-        !isnan(delta[2]) && !isinf(delta[2])) {
+        IS_REAL(delta[2])) {
         this->magBias[0] += delta[0];
         this->magBias[1] += delta[1];
         this->magBias[2] += delta[2];

flight/pios/inc/pios_math.h

@@ -26,6 +26,9 @@
 
 #ifndef PIOS_MATH_H
 #define PIOS_MATH_H
+
+#include <math.h>
+
 // Generic float math constants
 #define M_E_F        2.71828182845904523536028747135f      /* e */
 #define M_LOG2E_F    1.44269504088896340735992468100f      /* log_2 (e) */
@@ -66,20 +69,23 @@
 #define M_EULER_D    0.57721566490153286060651209008d      /* Euler constant */
 
 // Conversion macro
-#define RAD2DEG(rad)            ((rad) * (180.0f / M_PI_F))
+#define RAD2DEG(rad)      ((rad) * (180.0f / M_PI_F))
-#define DEG2RAD(deg)            ((deg) * (M_PI_F / 180.0f))
+#define DEG2RAD(deg)      ((deg) * (M_PI_F / 180.0f))
 
-#define RAD2DEG_D(rad)          ((rad) * (180.0d / M_PI_D))
+#define RAD2DEG_D(rad)    ((rad) * (180.0d / M_PI_D))
-#define DEG2RAD_D(deg)          ((deg) * (M_PI_D / 180.0d))
+#define DEG2RAD_D(deg)    ((deg) * (M_PI_D / 180.0d))
 
 // helper macros for LPFs
-#define LPF_ALPHA(dt, fc)       (dt / (dt + 1.0f / (2.0f * M_PI_F * fc)))
+#define LPF_ALPHA(dt, fc) (dt / (dt + 1.0f / (2.0f * M_PI_F * fc)))
 
 // Useful math macros
-#define MAX(a, b)               ((a) > (b) ? (a) : (b))
+#define MAX(a, b)         ((a) > (b) ? (a) : (b))
-#define MIN(a, b)               ((a) < (b) ? (a) : (b))
+#define MIN(a, b)         ((a) < (b) ? (a) : (b))
 
-#define IS_REAL(f)              (isfinite(f))
+__attribute__((optimize("no-fast-math"))) static inline int IS_REAL(float f)
+{
+    return !(isnan(f) || isinf(f));
+}
 
 // Bitfield access
 #define IS_SET(field, mask)     (((field) & (mask)) == (mask))