From 3e9294a2aeb8b7d2246965678d911b81ed7fffdd Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Wed, 15 Aug 2018 12:37:10 +1000
Subject: [PATCH] SITL: eliminate float-equals issues

---
 libraries/SITL/SIM_Aircraft.cpp      | 4 ++--
 libraries/SITL/SIM_FlightAxis.cpp    | 6 +++---
 libraries/SITL/SIM_Gripper_Servo.cpp | 2 +-
 libraries/SITL/SIM_XPlane.cpp        | 5 ++++-
 4 files changed, 10 insertions(+), 7 deletions(-)

diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp
index 1298ef1b9e..24b947a6c3 100644
--- a/libraries/SITL/SIM_Aircraft.cpp
+++ b/libraries/SITL/SIM_Aircraft.cpp
@@ -259,7 +259,7 @@ void Aircraft::setup_frame_time(float new_rate, float new_speedup)
 /* adjust frame_time calculation */
 void Aircraft::adjust_frame_time(float new_rate)
 {
-    if (rate_hz != new_rate) {
+    if (!is_equal(rate_hz, new_rate)) {
         rate_hz = new_rate;
         frame_time_us = static_cast<uint64_t>(1.0e6f/rate_hz);
         scaled_frame_time_us = frame_time_us/target_speedup;
@@ -421,7 +421,7 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
         }
     }
     
-    if (last_speedup != sitl->speedup && sitl->speedup > 0) {
+    if (!is_equal(last_speedup, float(sitl->speedup)) && sitl->speedup > 0) {
         set_speedup(sitl->speedup);
         last_speedup = sitl->speedup;
     }
diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp
index 1dab6f8a5a..7a092edd0d 100644
--- a/libraries/SITL/SIM_FlightAxis.cpp
+++ b/libraries/SITL/SIM_FlightAxis.cpp
@@ -428,7 +428,7 @@ void FlightAxis::update(const struct sitl_input &input)
                state.m_accelerationBodyAZ_MPS2);
 
     // accel on the ground is nasty in realflight, and prevents helicopter disarm
-    if (state.m_isTouchingGround) {
+    if (!is_zero(state.m_isTouchingGround)) {
         Vector3f accel_ef = (velocity_ef - last_velocity_ef) / dt_seconds;
         accel_ef.z -= GRAVITY_MSS;
         accel_body = dcm.transposed() * accel_ef;
@@ -441,7 +441,7 @@ void FlightAxis::update(const struct sitl_input &input)
     accel_body.z = constrain_float(accel_body.z, -a_limit, a_limit);
 
     // offset based on first position to account for offset in RF world
-    if (position_offset.is_zero() || state.m_resetButtonHasBeenPressed) {
+    if (position_offset.is_zero() || !is_zero(state.m_resetButtonHasBeenPressed)) {
         position_offset = position;
     }
     position -= position_offset;
@@ -492,7 +492,7 @@ void FlightAxis::update(const struct sitl_input &input)
 void FlightAxis::report_FPS(void)
 {
     if (frame_counter++ % 1000 == 0) {
-        if (last_frame_count_s != 0) {
+        if (!is_zero(last_frame_count_s)) {
             uint64_t frames = socket_frame_counter - last_socket_frame_counter;
             last_socket_frame_counter = socket_frame_counter;
             double dt = state.m_currentPhysicsTime_SEC - last_frame_count_s;
diff --git a/libraries/SITL/SIM_Gripper_Servo.cpp b/libraries/SITL/SIM_Gripper_Servo.cpp
index 5eafad74ee..5343069bc1 100644
--- a/libraries/SITL/SIM_Gripper_Servo.cpp
+++ b/libraries/SITL/SIM_Gripper_Servo.cpp
@@ -120,7 +120,7 @@ bool Gripper_Servo::should_report()
         return false;
     }
 
-    if (reported_position != position) {
+    if (!is_equal(reported_position, position)) {
         return true;
     }
 
diff --git a/libraries/SITL/SIM_XPlane.cpp b/libraries/SITL/SIM_XPlane.cpp
index 053cb74668..5dd90798bb 100644
--- a/libraries/SITL/SIM_XPlane.cpp
+++ b/libraries/SITL/SIM_XPlane.cpp
@@ -240,11 +240,14 @@ bool XPlane::receive_data(void)
                  * input from XPlane10
                  */
                 bool has_magic = ((uint32_t)(data[1] * throttle_magic_scale) % 1000U) == (uint32_t)(throttle_magic * throttle_magic_scale);
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wfloat-equal"
                 if (data[1] < 0 ||
                     data[1] == throttle_sent ||
                     has_magic) {
                     break;
                 }
+#pragma GCC diagnostic pop
                 rcin[2] = data[1];
             }
             break;
@@ -387,7 +390,7 @@ void XPlane::send_data(const struct sitl_input &input)
     if (SRV_Channels::find_channel(SRV_Channel::k_flap, flap_chan) ||
         SRV_Channels::find_channel(SRV_Channel::k_flap_auto, flap_chan)) {
         float flap = (input.servos[flap_chan]-1000)/1000.0;
-        if (flap != last_flap) {
+        if (!is_equal(flap, last_flap)) {
             send_dref("sim/flightmodel/controls/flaprqst", flap);
             send_dref("sim/aircraft/overflow/acf_flap_arm", flap>0?1:0);
         }