Re-send the RC config warnings once the GCS is connected, fixed compile warnings

1 files changed, 4 insertions, 3 deletions

diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp
index 9d79124e7..43f341ae7 100644
--- a/src/modules/commander/calibration_routines.cpp
+++ b/src/modules/commander/calibration_routines.cpp
@@ -40,6 +40,7 @@
  */
 
 #include <math.h>
+#include <float.h>
 
 #include "calibration_routines.h"
 
@@ -179,9 +180,9 @@ 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;
+		aA = (fabsf(aA) < FLT_EPSILON) ? 1.0f : aA;
+		aB = (fabsf(aB) < FLT_EPSILON) ? 1.0f : aB;
+		aC = (fabsf(aC) < FLT_EPSILON) ? 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);