diff options
| -rw-r--r-- | geoid.c | 5 | ||||
| -rw-r--r-- | gps.h | 3 | ||||
| -rw-r--r-- | gpsutils.c | 103 | ||||
| -rw-r--r-- | monitor_sirf.c | 5 |
4 files changed, 67 insertions, 49 deletions
@@ -90,9 +90,8 @@ void ecef_to_wgs84fix(struct gps_data_t *gpsdata, /* fill in WGS84 position/velocity fields from ECEF coordinates */ { double lambda,phi,p,theta,n,h,vnorth,veast,heading; - const double a = 6378137; /* equatorial radius */ - const double f = 1 / 298.257223563; /* flattening */ - const double b = a * (1 - f); /* polar radius */ + const double a = WGS84A; /* equatorial radius */ + const double b = WGS84B; /* polar radius */ const double e2 = (a*a - b*b) / (a*a); const double e_2 = (a*a - b*b) / (b*b); @@ -35,6 +35,9 @@ extern "C" { #define MAXCHANNELS 20 /* maximum GPS channels (*not* satellites!) */ #define GPS_PRNMAX 32 /* above this number are SBAS satellites */ +#define WGS84A 6378137 /* equatorial radius */ +#define WGS84F 298.257223563 /* flattening */ +#define WGS84B 6356752.3142 /* polar radius */ /* * The structure describing an uncertainty volume in kinematic space. * This is what GPSes are meant to produce; all the other info is @@ -254,52 +254,69 @@ void unix_to_gpstime(double unixtime, /*@out@*/int *week, /*@out@*/double *tow) #define Deg2Rad(n) ((n) * DEG_2_RAD) -static double CalcRad(double lat) -/* earth's radius of curvature in meters at specified latitude.*/ -{ - const double a = 6378.137; - const double e2 = 0.081082 * 0.081082; - // the radius of curvature of an ellipsoidal Earth in the plane of a - // meridian of latitude is given by - // - // R' = a * (1 - e^2) / (1 - e^2 * (sin(lat))^2)^(3/2) - // - // where a is the equatorial radius, - // b is the polar radius, and - // e is the eccentricity of the ellipsoid = sqrt(1 - b^2/a^2) - // - // a = 6378 km (3963 mi) Equatorial radius (surface to center distance) - // b = 6356.752 km (3950 mi) Polar radius (surface to center distance) - // e = 0.081082 Eccentricity - double sc = sin(Deg2Rad(lat)); - double x = a * (1.0 - e2); - double z = 1.0 - e2 * sc * sc; - double y = pow(z, 1.5); - double r = x / y; - - return r * 1000.0; // Convert to meters -} - double earth_distance(double lat1, double lon1, double lat2, double lon2) /* distance in meters between two points specified in degrees. */ { - double x1 = CalcRad(lat1) * cos(Deg2Rad(lon1)) * sin(Deg2Rad(90-lat1)); - double x2 = CalcRad(lat2) * cos(Deg2Rad(lon2)) * sin(Deg2Rad(90-lat2)); - double y1 = CalcRad(lat1) * sin(Deg2Rad(lon1)) * sin(Deg2Rad(90-lat1)); - double y2 = CalcRad(lat2) * sin(Deg2Rad(lon2)) * sin(Deg2Rad(90-lat2)); - double z1 = CalcRad(lat1) * cos(Deg2Rad(90-lat1)); - double z2 = CalcRad(lat2) * cos(Deg2Rad(90-lat2)); - double a = (x1*x2 + y1*y2 + z1*z2)/pow(CalcRad((lat1+lat2)/2),2); - - // a should be in [1, -1] but can sometimes fall outside it by - // a very small amount due to rounding errors in the preceding - // calculations. This is prone to happen when the argument points - // are very close together. Return NaN so calculations trying - // to use this will also blow up. - if (fabs(a) > 1) - return NAN; - else - return CalcRad((lat1+lat2) / 2) * acos(a); + /* + * this is a translation of the javascript implementation of the + * Vincenty distance formula by Chris Veness. See + * http://www.movable-type.co.uk/scripts/latlong-vincenty.html + */ + double a, b, f; // WGS-84 ellipsoid params + double L, L_P, U1, U2, s_U1, c_U1, s_U2, c_U2; + double uSq, A, B, d_S, lambda; + double s_L, c_L, s_S, C; + double c_S, S, s_A, c_SqA, c_2SM; + int i = 100; + + a = WGS84A; + b = WGS84B; + f = 1/WGS84F; + L = Deg2Rad(lon2 - lon1); + U1 = atan((1 - f) * tan(Deg2Rad(lat1))); + U2 = atan((1 - f) * tan(Deg2Rad(lat2))); + s_U1 = sin(U1); + c_U1 = cos(U1); + s_U2 = sin(U2); + c_U2 = cos(U2); + lambda = L; + + do { + s_L = sin(lambda); + c_L = cos(lambda); + s_S = sqrt((c_U2 * s_L) * (c_U2 * s_L) + + (c_U1 * s_U2 - s_U1 * c_U2 * c_L) * + (c_U1 * s_U2 - s_U1 * c_U2 * c_L)); + + if (s_S==0) + return 0; + + c_S = s_U1 * s_U2 + c_U1 * c_U2 * c_L; + S = atan2(s_S, c_S); + s_A = c_U1 * c_U2 * s_L / s_S; + c_SqA = 1 - s_A * s_A; + c_2SM = c_S - 2 * s_U1 * s_U2 / c_SqA; + + if (isnan(c_2SM)) + c_2SM = 0; + + C = f / 16 * c_SqA * (4 + f * (4 - 3 * c_SqA)); + L_P = lambda; + lambda = L + (1 - C) * f * s_A * + (S + C * s_S * (c_2SM + C * c_S * (2 * c_2SM * c_2SM - 1))); + } while ((fabs(lambda - L_P) > 1.0e-12) && (--i > 0)); + + if (i == 0) + return NAN; // formula failed to converge + + uSq = c_SqA * ((a*a) - (b*b)) / (b*b); + A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq))); + B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq))); + d_S = B * s_S * (c_2SM + B/4 * + (c_S * (-1 + 2 * c_2SM * c_2SM) - B/6 * c_2SM * + (-3 + 4 * s_S * s_S) * (-3 + 4 * c_2SM * c_2SM))); + + return(WGS84B * A * (S - d_S)); } /***************************************************************************** diff --git a/monitor_sirf.c b/monitor_sirf.c index 685ee592..0c012772 100644 --- a/monitor_sirf.c +++ b/monitor_sirf.c @@ -235,9 +235,8 @@ static void decode_time(int week, int tow) static void decode_ecef(double x, double y, double z, double vx, double vy, double vz) { - const double a = 6378137; - const double f = 1 / 298.257223563; - const double b = a * (1 - f); + const double a = WGS84A; + const double b = WGS84B; const double e2 = (a*a - b*b) / (a*a); const double e_2 = (a*a - b*b) / (b*b); double lambda,p,theta,phi,n,h,vnorth,veast,vup,speed,heading; |