diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-30 12:19:08 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-30 12:19:08 +0200 |
commit | 3f6851b9879c2e4d764926a3bc29ff800a17b73d (patch) | |
tree | 0d1e2741f1d42b011ed6085bb5beabda64e354e8 /src/modules/commander/calibration_routines.cpp | |
parent | 8e41cbfdfa2083c421954b6d9aadde5c0e508cdc (diff) | |
download | px4-firmware-3f6851b9879c2e4d764926a3bc29ff800a17b73d.tar.gz px4-firmware-3f6851b9879c2e4d764926a3bc29ff800a17b73d.tar.bz2 px4-firmware-3f6851b9879c2e4d764926a3bc29ff800a17b73d.zip |
Re-send the RC config warnings once the GCS is connected, fixed compile warnings
Diffstat (limited to 'src/modules/commander/calibration_routines.cpp')
-rw-r--r-- | src/modules/commander/calibration_routines.cpp | 7 |
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); |