Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 20:48:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: add casts to consider literals as doubles

Browse Source 
Since we pass -fsingle-precision-constant to the compiler, add casts to
make literals real doubles.
This commit is contained in:
Lucas De Marchi 2017-01-31 09:19:09 -08:00
parent 48d94db259
commit 87038d8ef1
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_Math/location.cpp
View File

@ -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;