diff --git a/libraries/AP_Math/location.cpp b/libraries/AP_Math/location.cpp index 17dffdeb8b..bb8072bce6 100644 --- a/libraries/AP_Math/location.cpp +++ b/libraries/AP_Math/location.cpp @@ -202,7 +202,7 @@ void print_latlon(AP_HAL::BetterStream *s, int32_t lat_or_lon) void wgsllh2ecef(const Vector3d &llh, Vector3d &ecef) { double d = WGS84_E * sin(llh[0]); - double N = WGS84_A / sqrt(1. - d*d); + double N = WGS84_A / sqrt(1 - d*d); ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]); ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]); @@ -222,7 +222,7 @@ void wgsecef2llh(const Vector3d &ecef, Vector3d &llh) { /* If we are close to the pole then convergence is very slow, treat this is a * special case. */ - if (p < WGS84_A*1e-16) { + if (p < WGS84_A * double(1e-16)) { llh[0] = copysign(M_PI_2, ecef[2]); llh[2] = fabs(ecef[2]) - WGS84_B; return; @@ -230,7 +230,7 @@ void wgsecef2llh(const Vector3d &ecef, Vector3d &llh) { /* Calculate some other constants as defined in the Fukushima paper. */ const double P = p / WGS84_A; - const double e_c = sqrt(1. - WGS84_E*WGS84_E); + const double e_c = sqrt(1 - WGS84_E*WGS84_E); const double Z = fabs(ecef[2]) * e_c / WGS84_A; /* Initial values for S and C correspond to a zero height solution. */ @@ -253,7 +253,7 @@ void wgsecef2llh(const Vector3d &ecef, Vector3d &llh) { A_n = sqrt(S*S + C*C); D_n = Z*A_n*A_n*A_n + WGS84_E*WGS84_E*S*S*S; F_n = P*A_n*A_n*A_n - WGS84_E*WGS84_E*C*C*C; - B_n = 1.5*WGS84_E*S*C*C*(A_n*(P*S - Z*C) - WGS84_E*S*C); + B_n = double(1.5) * WGS84_E*S*C*C*(A_n*(P*S - Z*C) - WGS84_E*S*C); /* Update step. */ S = D_n*F_n - B_n*S; @@ -289,7 +289,7 @@ void wgsecef2llh(const Vector3d &ecef, Vector3d &llh) { } /* Check for convergence and exit early if we have converged. */ - if (fabs(S - prev_S) < 1e-16 && fabs(C - prev_C) < 1e-16) { + if (fabs(S - prev_S) < double(1e-16) && fabs(C - prev_C) < double(1e-16)) { break; } else { prev_S = S;