How to handle floating point comparison warnings (DO NOT MERGE)

makefiles/toolchain_gnu-arm-eabi.mk

@@ -129,7 +129,7 @@ ARCHWARNINGS		 = -Wall \
 			   -Wextra \
 			   -Wdouble-promotion \
 			   -Wshadow \
-			   -Wfloat-equal \
+			   -Werror=float-equal \
 			   -Wframe-larger-than=1024 \
 			   -Wpointer-arith \
 			   -Wlogical-op \
@@ -143,6 +143,7 @@ ARCHWARNINGS		 = -Wall \
 #   -Wcast-qual  - generates spurious noreturn attribute warnings, try again later
 #   -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code
 #   -Wcast-align - would help catch bad casts in some cases, but generates too many false positives
+ARCHWARNINGS = -Werror=float-equal
 
 # C-specific warnings
 #

src/examples/flow_position_estimator/flow_position_estimator_main.c

@@ -69,6 +69,7 @@
 #include <poll.h>
 
 #include "flow_position_estimator_params.h"
+#include "fp_helpers.h"
 
 __EXPORT int flow_position_estimator_main(int argc, char *argv[]);
 static bool thread_should_exit = false;		/**< Daemon exit flag */
@@ -377,7 +378,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
 					{
 						/* simple lowpass sonar filtering */
 						/* if new value or with sonar update frequency */
-						if (sonar_new != sonar_last || counter % 10 == 0)
+						if (!is_equal_float(sonar_new, sonar_last) || counter % 10 == 0)
 						{
 							sonar_lp = 0.05f * sonar_new + 0.95f * sonar_lp;
 							sonar_last = sonar_new;

src/include/fp_helpers.h

@@ -0,0 +1,49 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file fp_helpers.h
+ */
+
+#ifndef __FP_HELPERS_H
+#define __FP_HELPERS_H
+
+#include "float.h"
+#include "math.h"
+
+#define is_exactly_zero_float(f)	(!isfinite(1.0f/(f)))
+#define is_exactly_zero_double(d)	(!isfinite(1.0/(d)))
+#define is_equal_float(f1, f2)		(fabsf((f1)- (f2)) > FLT_EPSILON)
+#define is_equal_double(d1, d2)		(fabs((d1)- (d2)) > DBL_EPSILON)
+
+#endif

src/lib/geo/geo.c

@@ -50,6 +50,8 @@
 #include <math.h>
 #include <stdbool.h>
 
+#include "fp_helpers.h"
+
 /*
  * Azimuthal Equidistant Projection
  * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html
@@ -74,7 +76,7 @@ __EXPORT void map_projection_project(struct map_projection_reference_s *ref, dou
 	double cos_d_lon = cos(lon_rad - ref->lon);
 
 	double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon);
-	double k = (c == 0.0) ? 1.0 : (c / sin(c));
+	double k = is_equal_double(c, 0.0) ? 1.0 : (c / sin(c));
 
 	*x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH;
 	*y = k * cos_lat * sin(lon_rad - ref->lon) * CONSTANTS_RADIUS_OF_EARTH;
@@ -91,7 +93,7 @@ __EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, f
 	double lat_rad;
 	double lon_rad;
 
-	if (c != 0.0) {
+	if (is_equal_double(c, 0.0)) {
 		lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c);
 		lon_rad = (ref->lon + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
 
@@ -197,7 +199,10 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d
 	crosstrack_error->bearing = 0.0f;
 
 	// Return error if arguments are bad
-	if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) { return return_value; }
+	if (is_exactly_zero_double(lat_now) || is_exactly_zero_double(lon_now) || is_exactly_zero_double(lat_start) || is_exactly_zero_double(lon_start) || is_exactly_zero(lat_end) || is_exactly_zero_double(lon_end))
+	{
+		return return_value;
+	}
 
 	bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
 	bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
@@ -248,8 +253,10 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
 	crosstrack_error->bearing = 0.0f;
 
 	// Return error if arguments are bad
-	if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) { return return_value; }
-
+	if (is_exactly_zero_deouble(lat_now) || is_exactly_zero_deouble(lon_now) || is_exactly_zero_deouble(lat_center) || is_exactly_zero_deouble(lon_center) || is_exactly_zero_deouble(radius))
+	{
+		return return_value;
+	}
 
 	if (arc_sweep >= 0) {
 		bearing_sector_start = arc_start_bearing;

src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c

@@ -15,6 +15,7 @@
 #include "cross.h"
 #include "eye.h"
 #include "mrdivide.h"
+#include "fp_helpers.h"
 
 /* Type Definitions */
 
@@ -49,7 +50,7 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1)
     }
 
     y = (real32_T)atan2((real32_T)b_u0, (real32_T)b_u1);
-  } else if (u1 == 0.0F) {
+  } else if (is_equal_float(u1, 0.0f)) {
     if (u0 > 0.0F) {
       y = RT_PIF / 2.0F;
     } else if (u0 < 0.0F) {

src/modules/attitude_estimator_ekf/codegen/mrdivide.c

@@ -11,6 +11,7 @@
 #include "rt_nonfinite.h"
 #include "attitudeKalmanfilter.h"
 #include "mrdivide.h"
+#include "fp_helpers.h"
 
 /* Type Definitions */
 
@@ -142,7 +143,7 @@ void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
       }
     }
 
-    if (b_A[c + iy] != 0.0F) {
+    if (!is_equal_float(b_A[c + iy], 0.0f)) {
       if (iy != 0) {
         ipiv[j] = (int8_T)((j + iy) + 1);
         ix = j;
@@ -166,7 +167,7 @@ void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
     jy = c + 6;
     for (k = 1; k <= 5 - j; k++) {
       temp = b_A[jy];
-      if (b_A[jy] != 0.0F) {
+      if (!is_equal_float(b_A[jy], 0.0f)) {
         ix = c + 1;
         i3 = (iy - j) + 12;
         for (ijA = 7 + iy; ijA + 1 <= i3; ijA++) {
@@ -200,7 +201,7 @@ void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
     c = 6 * j;
     for (k = 0; k < 6; k++) {
       iy = 6 * k;
-      if (Y[k + c] != 0.0F) {
+      if (!is_equal_float(Y[k + c], 0.0f)) {
         for (jy = k + 2; jy < 7; jy++) {
           Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];
         }
@@ -212,7 +213,7 @@ void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
     c = 6 * j;
     for (k = 5; k > -1; k += -1) {
       iy = 6 * k;
-      if (Y[k + c] != 0.0F) {
+      if (is_equal_float(Y[k + c], 0.0f)) {
         Y[k + c] /= b_A[k + iy];
         for (jy = 0; jy + 1 <= k; jy++) {
           Y[jy + c] -= Y[k + c] * b_A[jy + iy];
@@ -268,7 +269,7 @@ void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
       }
     }
 
-    if (b_A[c + iy] != 0.0F) {
+    if (!is_equal_float(b_A[c + iy], 0.0f)) {
       if (iy != 0) {
         ipiv[j] = (int8_T)((j + iy) + 1);
         ix = j;
@@ -292,7 +293,7 @@ void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
     jy = c + 9;
     for (k = 1; k <= 8 - j; k++) {
       temp = b_A[jy];
-      if (b_A[jy] != 0.0F) {
+      if (!is_equal_float(b_A[jy], 0.0f)) {
         ix = c + 1;
         i2 = (iy - j) + 18;
         for (ijA = 10 + iy; ijA + 1 <= i2; ijA++) {
@@ -326,7 +327,7 @@ void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
     c = 9 * j;
     for (k = 0; k < 9; k++) {
       iy = 9 * k;
-      if (Y[k + c] != 0.0F) {
+      if (!is_equal_float(Y[k + c], 0.0f)) {
         for (jy = k + 2; jy < 10; jy++) {
           Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];
         }
@@ -338,7 +339,7 @@ void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
     c = 9 * j;
     for (k = 8; k > -1; k += -1) {
       iy = 9 * k;
-      if (Y[k + c] != 0.0F) {
+      if (!is_equal_float(Y[k + c], 0.0f)) {
         Y[k + c] /= b_A[k + iy];
         for (jy = 0; jy + 1 <= k; jy++) {
           Y[jy + c] -= Y[k + c] * b_A[jy + iy];

src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c

@@ -15,6 +15,7 @@
 #include "rt_nonfinite.h"
 #include "rtGetNaN.h"
 #include "rtGetInf.h"
+#include "math.h"
 
 real_T rtInf;
 real_T rtMinusInf;
@@ -45,7 +46,7 @@ void rt_InitInfAndNaN(size_t realSize)
  */
 boolean_T rtIsInf(real_T value)
 {
-  return ((value==rtInf || value==rtMinusInf) ? 1U : 0U);
+	return isfinite(value);
 }
 
 /* Function: rtIsInfF =================================================
@@ -54,7 +55,7 @@ boolean_T rtIsInf(real_T value)
  */
 boolean_T rtIsInfF(real32_T value)
 {
-  return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U);
+	return !isfinite(value);
 }
 
 /* Function: rtIsNaN ==================================================
@@ -63,11 +64,7 @@ boolean_T rtIsInfF(real32_T value)
  */
 boolean_T rtIsNaN(real_T value)
 {
-#if defined(_MSC_VER) && (_MSC_VER <= 1200)
-  return _isnan(value)? TRUE:FALSE;
-#else
-  return (value!=value)? 1U:0U;
-#endif
+	return isnan(value);
 }
 
 /* Function: rtIsNaNF =================================================
@@ -76,11 +73,7 @@ boolean_T rtIsNaN(real_T value)
  */
 boolean_T rtIsNaNF(real32_T value)
 {
-#if defined(_MSC_VER) && (_MSC_VER <= 1200)
-  return _isnan((real_T)value)? true:false;
-#else
-  return (value!=value)? 1U:0U;
-#endif
+	return isnan(value);
 }
 
 

src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp

@@ -77,6 +77,8 @@
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
 
+#include "fp_helpers.h"
+
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -267,7 +269,7 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl
 	}
 
 	//! If magnetometer measurement is available, use it.
-	if(!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) {
+	if(!(is_exactly_zero_float(mx) && is_exactly_zero_float(my) && is_exactly_zero_float(mz))) {
 		float hx, hy, hz, bx, bz;
 		float halfwx, halfwy, halfwz;
 
@@ -297,7 +299,7 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl
 	}
 
 	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
-	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+	if(!(is_exactly_zero_float(ax) && is_exactly_zero_float(ay) && is_exactly_zero_float(az))) {
 		float halfvx, halfvy, halfvz;
 
 		// Normalise accelerometer measurement
@@ -318,7 +320,7 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl
 	}
 
 	// Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
-	if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
+	if(!is_exactly_zero_float(halfex) && !is_exactly_zero_float(halfey) && !is_exactly_zero_float(halfez)) {
 		// Compute and apply integral feedback if enabled
 		if(twoKi > 0.0f) {
 			gyro_bias[0] += twoKi * halfex * dt;	// integral error scaled by Ki

src/modules/commander/accelerometer_calibration.cpp

@@ -124,6 +124,7 @@
 #include "accelerometer_calibration.h"
 #include "calibration_messages.h"
 #include "commander_helper.h"
+#include "fp_helpers.h"
 
 #include <unistd.h>
 #include <stdio.h>
@@ -524,7 +525,8 @@ int mat_invert3(float src[3][3], float dst[3][3])
 		    src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
 		    src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
 
-	if (det == 0.0f) {
+	// FIXME: Check this
+	if (is_exactly_zero_float(det)) {
 		return ERROR;        // Singular matrix
 	}
 

src/modules/commander/calibration_routines.cpp

@@ -42,6 +42,7 @@
 #include <math.h>
 
 #include "calibration_routines.h"
+#include "fp_helpers.h"
 
 
 int sphere_fit_least_squares(const float x[], const float y[], const float z[],
@@ -179,9 +180,10 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
 		aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2);
 		aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2);
 		aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2);
-		aA = (aA == 0.0f) ? 1.0f : aA;
-		aB = (aB == 0.0f) ? 1.0f : aB;
-		aC = (aC == 0.0f) ? 1.0f : aC;
+		// FIXME: Double check alogorithm
+		aA = is_exactly_zero_float(aA) ? 1.0f : aA;
+		aB = is_exactly_zero_float(aB) ? 1.0f : aB;
+		aC = is_exactly_zero_float(aC) ? 1.0f : aC;
 
 		//Compute next iteration
 		nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA);

src/modules/commander/commander.cpp

@@ -101,6 +101,7 @@
 #include "baro_calibration.h"
 #include "rc_calibration.h"
 #include "airspeed_calibration.h"
+#include "fp_helpers.h"
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -469,7 +470,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
 			// Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
 			// We use an float epsilon delta to test float equality.
-			if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) {
+			if (!is_exactly_zero_float(cmd->param1) && is_equal_float(cmd->param1, 1.0f)) {
 				mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1);
 
 			} else {
@@ -479,7 +480,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 					status->arming_state = ARMING_STATE_IN_AIR_RESTORE;
 				}
 
-				transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command");
+				transition_result_t arming_res = arm_disarm(!is_exactly_zero_float(cmd->param1), mavlink_fd, "arm/disarm component command");
 
 				if (arming_res == TRANSITION_DENIED) {
 					mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");