From f808e8d7fe8e4edc7997b2eed163d94f921a0b79 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 29 Jun 2014 19:50:16 -0700 Subject: [PATCH 1/3] commit --- src/include/fp_helpers.h | 57 +++++++++++++++++++ .../attitude_estimator_so3_main.cpp | 8 ++- 2 files changed, 62 insertions(+), 3 deletions(-) create mode 100644 src/include/fp_helpers.h diff --git a/src/include/fp_helpers.h b/src/include/fp_helpers.h new file mode 100644 index 000000000000..05e42e99fedc --- /dev/null +++ b/src/include/fp_helpers.h @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * 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 + +inline bool is_exactly_zero_float(float f) +{ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" + return f == 0.0f; +#pragma GCC diagnostic pop +} + +inline bool is_exactly_zero_double(double d) +{ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" + return d == 0.0; +#pragma GCC diagnostic pop +} + +#endif diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index e55b01160864..f7a0d34af979 100755 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -77,6 +77,8 @@ #include #include +#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 From a055759934e4dfed05866a490a708a303edcd223 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 30 Jun 2014 18:30:39 -0700 Subject: [PATCH 2/3] commit --- makefiles/toolchain_gnu-arm-eabi.mk | 3 ++- .../flow_position_estimator_main.c | 3 ++- src/include/fp_helpers.h | 20 ++++++------------- src/lib/geo/geo.c | 15 +++++++++----- .../codegen/attitudeKalmanfilter.c | 3 ++- .../attitude_estimator_ekf/codegen/mrdivide.c | 17 ++++++++-------- .../codegen/rt_nonfinite.c | 17 +++++----------- .../commander/accelerometer_calibration.cpp | 4 +++- .../commander/calibration_routines.cpp | 8 +++++--- src/modules/commander/commander.cpp | 5 +++-- .../ekf_att_pos_estimator_main.cpp | 8 ++++---- .../fw_pos_control_l1_main.cpp | 3 ++- src/modules/navigator/mission_block.cpp | 4 ++-- .../position_estimator_inav_main.c | 3 ++- src/modules/sensors/sensors.cpp | 4 +++- src/systemcmds/tests/test_float.c | 19 ++++++++++-------- 16 files changed, 71 insertions(+), 65 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 808b635bb822..f49f826c9b81 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/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 # diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index c775428ef13d..1aadc2daf9bc 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -69,6 +69,7 @@ #include #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; diff --git a/src/include/fp_helpers.h b/src/include/fp_helpers.h index 05e42e99fedc..0cadc3b1ebbb 100644 --- a/src/include/fp_helpers.h +++ b/src/include/fp_helpers.h @@ -38,20 +38,12 @@ #ifndef __FP_HELPERS_H #define __FP_HELPERS_H -inline bool is_exactly_zero_float(float f) -{ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wfloat-equal" - return f == 0.0f; -#pragma GCC diagnostic pop -} +#include "float.h" +#include "math.h" -inline bool is_exactly_zero_double(double d) -{ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wfloat-equal" - return d == 0.0; -#pragma GCC diagnostic pop -} +#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 diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 9a24ff50e287..a862e83b86b2 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -74,7 +74,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 +91,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_zero_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 +197,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 +251,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; diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c index 9e97ad11a8c4..0cbf510f60ae 100755 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c +++ b/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) { diff --git a/src/modules/attitude_estimator_ekf/codegen/mrdivide.c b/src/modules/attitude_estimator_ekf/codegen/mrdivide.c index a810f22e4fc0..43a8b3c6c097 100755 --- a/src/modules/attitude_estimator_ekf/codegen/mrdivide.c +++ b/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]; diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c index 303d1d9d2ef1..2ce2288dc7d6 100755 --- a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c +++ b/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); } diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 7180048ff2af..e1571f66fa5e 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/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 #include @@ -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 } diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index be38ea104602..0e4d8c5cbba0 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -42,6 +42,7 @@ #include #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); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bb42889ea0d3..14d5353a6a17 100644 --- a/src/modules/commander/commander.cpp +++ b/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"); diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index e4f805dc04d6..8f9683493a40 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -84,7 +84,7 @@ #include #include "estimator_23states.h" - +#include "fp_helpers.h" /** @@ -1256,9 +1256,9 @@ FixedwingEstimator::task_main() float gps_dt = (_gps.timestamp_position - last_gps) / 1e6f; // Calculate acceleration predicted by GPS velocity change - if (((fabsf(_ekf->velNED[0] - _gps.vel_n_m_s) > FLT_EPSILON) || - (fabsf(_ekf->velNED[1] - _gps.vel_e_m_s) > FLT_EPSILON) || - (fabsf(_ekf->velNED[2] - _gps.vel_d_m_s) > FLT_EPSILON)) && (gps_dt > 0.00001f)) { + if (!is_equal_float(_ekf->velNED[0],_gps.vel_n_m_s) || + !is_equal_float(_ekf->velNED[1], _gps.vel_e_m_s) || + !is_equal_float(_ekf->velNED[2], _gps.vel_d_m_s) && (gps_dt > 0.00001f)) { _ekf->accelGPSNED[0] = (_ekf->velNED[0] - _gps.vel_n_m_s) / gps_dt; _ekf->accelGPSNED[1] = (_ekf->velNED[1] - _gps.vel_e_m_s) / gps_dt; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 000c02e3dad0..570453fffad7 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -91,6 +91,7 @@ #include #include "landingslope.h" #include "mtecs/mTecs.h" +#include "fp_helpers.h" /** @@ -1378,7 +1379,7 @@ FixedwingPositionControl::task_main() float turn_distance = _l1_control.switch_distance(100.0f); /* lazily publish navigation capabilities */ - if (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0) { + if (is_equal_float(turn_distance, _nav_capabilities.turn_distance) && turn_distance > 0.0f) { /* set new turn distance */ _nav_capabilities.turn_distance = turn_distance; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 9b8d3d9c709f..ad9b18b54324 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -50,7 +50,7 @@ #include "navigator.h" #include "mission_block.h" - +#include "fp_helpers.h" MissionBlock::MissionBlock(Navigator *navigator, const char *name) : NavigatorMode(navigator, name), @@ -222,7 +222,7 @@ MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet } if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER - || pos_sp_triplet->current.loiter_radius != _navigator->get_loiter_radius() + || !is_equal_float(pos_sp_triplet->current.loiter_radius, _navigator->get_loiter_radius()) || pos_sp_triplet->current.loiter_direction != 1 || pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 62cee145e5d3..8a7f3288d47d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -69,6 +69,7 @@ #include "position_estimator_inav_params.h" #include "inertial_filter.h" +#include "fp_helpers.h" #define MIN_VALID_W 0.00001f #define PUB_INTERVAL 10000 // limit publish rate to 100 Hz @@ -475,7 +476,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; flow_prev = flow.flow_timestamp; - if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) { + if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && !is_equal_float(flow.ground_distance_m, sonar_prev)) { sonar_time = t; sonar_prev = flow.ground_distance_m; corr_sonar = flow.ground_distance_m + surface_offset + z_est[0]; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 16fcb4c26d39..7e7247cab196 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -83,6 +83,8 @@ #include #include +#include "fp_helpers.h" + #define GYRO_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */ #define ACC_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */ #define MAGN_HEALTH_COUNTER_LIMIT_ERROR 100 /* 1000 ms downtime at 100 Hz update rate */ @@ -1315,7 +1317,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) * a valid voltage from a connected sensor. Also assume a non- * zero offset from the sensor if its connected. */ - if (voltage > 0.4f && _parameters.diff_pres_analog_enabled) { + if (voltage > 0.4f && !is_exactly_zero_float(_parameters.diff_pres_analog_enabled)) { float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor diff --git a/src/systemcmds/tests/test_float.c b/src/systemcmds/tests/test_float.c index 4921c9bbbd87..5d0a5a20ad87 100644 --- a/src/systemcmds/tests/test_float.c +++ b/src/systemcmds/tests/test_float.c @@ -49,6 +49,8 @@ #include #include +#include "fp_helpers.h" + typedef union { float f; double d; @@ -66,7 +68,6 @@ int test_float(int argc, char *argv[]) float sinf_zero = sinf(0.0f); float sinf_one = sinf(1.0f); - float sqrt_two = sqrt(2.0f); if (sinf_zero == 0.0f) { printf("\t success: sinf(0.0f) == 0.0f\n"); @@ -78,7 +79,7 @@ int test_float(int argc, char *argv[]) fflush(stdout); - if (fabsf((sinf_one - 0.841470956802368164062500000000f)) < FLT_EPSILON) { + if (is_equal_float(sinf_one, 0.841470956802368164062500000000f)) { printf("\t success: sinf(1.0f) == 0.84147f\n"); } else { @@ -90,7 +91,7 @@ int test_float(int argc, char *argv[]) float asinf_one = asinf(1.0f); - if (fabsf((asinf_one - 1.570796251296997070312500000000f)) < FLT_EPSILON * 1.5f) { + if (is_equal_float(asinf_one, 1.570796251296997070312500000000f)) { printf("\t success: asinf(1.0f) == 1.57079f\n"); } else { @@ -102,7 +103,7 @@ int test_float(int argc, char *argv[]) float cosf_one = cosf(1.0f); - if (fabsf((cosf_one - 0.540302336215972900390625000000f)) < FLT_EPSILON) { + if (is_equal_float(cosf_one, 0.540302336215972900390625000000f)) { printf("\t success: cosf(1.0f) == 0.54030f\n"); } else { @@ -115,7 +116,7 @@ int test_float(int argc, char *argv[]) float acosf_one = acosf(1.0f); - if (fabsf((acosf_one - 0.000000000000000000000000000000f)) < FLT_EPSILON) { + if (is_equal_float(acosf_one, 0.000000000000000000000000000000f)) { printf("\t success: acosf(1.0f) == 0.0f\n"); } else { @@ -128,7 +129,7 @@ int test_float(int argc, char *argv[]) float sinf_zero_one = sinf(0.1f); - if (fabs(sinf_zero_one - 0.0998334166f) < FLT_EPSILON) { + if (is_equal_float(sinf_zero_one, 0.0998334166f)) { printf("\t success: sinf(0.1f) == 0.09983f\n"); } else { @@ -136,7 +137,9 @@ int test_float(int argc, char *argv[]) ret = -2; } - if (sqrt_two == 1.41421356f) { + float sqrt_two = sqrt(2.0f); + + if (is_equal_float(sqrt_two, 1.41421356f)) { printf("\t success: sqrt(2.0f) == 1.41421f\n"); } else { @@ -146,7 +149,7 @@ int test_float(int argc, char *argv[]) float atan2f_ones = atan2(1.0f, 1.0f); - if (fabsf(atan2f_ones - 0.785398163397448278999490867136f) < FLT_EPSILON) { + if (is_equal_float(atan2f_ones, 0.785398163397448278999490867136f)) { printf("\t success: atan2f(1.0f, 1.0f) == 0.78539f\n"); } else { From 41e262ca884ebcbfdfddb484be3a500dc6963664 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 30 Jun 2014 19:24:37 -0700 Subject: [PATCH 3/3] commit --- src/lib/geo/geo.c | 4 +++- src/systemcmds/tests/test_bson.c | 3 ++- src/systemcmds/tests/test_float.c | 13 ++++++++----- 3 files changed, 13 insertions(+), 7 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index a862e83b86b2..3382f26eabc3 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -50,6 +50,8 @@ #include #include +#include "fp_helpers.h" + /* * Azimuthal Equidistant Projection * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html @@ -91,7 +93,7 @@ __EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, f double lat_rad; double lon_rad; - if (is_equal_zero_double(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)); diff --git a/src/systemcmds/tests/test_bson.c b/src/systemcmds/tests/test_bson.c index 6130fe7635cb..a05bd5d4a3bc 100644 --- a/src/systemcmds/tests/test_bson.c +++ b/src/systemcmds/tests/test_bson.c @@ -45,6 +45,7 @@ #include #include "tests.h" +#include "fp_helpers.h" static const bool sample_bool = true; static const int32_t sample_small_int = 123; @@ -123,7 +124,7 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node) warnx("FAIL: decoder: double1 type %d, expected %d", node->type, BSON_DOUBLE); return 1; } - if (node->d != sample_double) { + if (!is_equal_double(node->d, sample_double)) { warnx("FAIL: decoder: double1 value %f, expected %f", node->d, sample_double); return 1; } diff --git a/src/systemcmds/tests/test_float.c b/src/systemcmds/tests/test_float.c index 5d0a5a20ad87..51f105c67b05 100644 --- a/src/systemcmds/tests/test_float.c +++ b/src/systemcmds/tests/test_float.c @@ -69,7 +69,7 @@ int test_float(int argc, char *argv[]) float sinf_zero = sinf(0.0f); float sinf_one = sinf(1.0f); - if (sinf_zero == 0.0f) { + if (is_equal_float(sinf_zero, 0.0f)) { printf("\t success: sinf(0.0f) == 0.0f\n"); } else { @@ -191,7 +191,7 @@ int test_float(int argc, char *argv[]) double d1d2 = d1 * d2; - if (d1d2 == 2.022200000000000219557705349871) { + if (is_equal_double(d1d2, 2.022200000000000219557705349871)) { printf("\t success: 1.0111 * 2.0 == 2.0222\n"); } else { @@ -204,7 +204,10 @@ int test_float(int argc, char *argv[]) // Assign value of f1 to d1 d1 = f1; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" if (f1 == (float)d1) { +#pragma GCC diagnostic pop printf("\t success: (float) 1.55f == 1.55 (double)\n"); } else { @@ -219,7 +222,7 @@ int test_float(int argc, char *argv[]) double sin_one = sin(1.0); double atan2_ones = atan2(1.0, 1.0); - if (sin_zero == 0.0) { + if (is_equal_double(sin_zero, 0.0)) { printf("\t success: sin(0.0) == 0.0\n"); } else { @@ -227,7 +230,7 @@ int test_float(int argc, char *argv[]) ret = -9; } - if (sin_one == 0.841470984807896504875657228695) { + if (is_equal_double(sin_one, 0.841470984807896504875657228695)) { printf("\t success: sin(1.0) == 0.84147098480\n"); } else { @@ -235,7 +238,7 @@ int test_float(int argc, char *argv[]) ret = -10; } - if (atan2_ones != 0.785398) { + if (!is_equal_double(atan2_ones, 0.785398)) { printf("\t success: atan2(1.0, 1.0) == 0.785398\n"); } else {