summaryrefslogtreecommitdiff
path: root/geoid.c
diff options
context:
space:
mode:
authorEric S. Raymond <esr@thyrsus.com>2010-04-14 05:34:17 -0400
committerEric S. Raymond <esr@thyrsus.com>2010-04-14 05:34:17 -0400
commite37de01df5aaf6161cd88424cb522f590355f5d8 (patch)
tree58100ce4ee258635e5cf3bd5a31f97c835ed7716 /geoid.c
parent29f16317f536d8f525bff3a3b8f9541076fab550 (diff)
downloadgpsd-e37de01df5aaf6161cd88424cb522f590355f5d8.tar.gz
Reindented some small utility modules. All regression tests pass.
Diffstat (limited to 'geoid.c')
-rw-r--r--geoid.c113
1 files changed, 63 insertions, 50 deletions
diff --git a/geoid.c b/geoid.c
index 0e1647a6..66b72630 100644
--- a/geoid.c
+++ b/geoid.c
@@ -14,21 +14,28 @@
static double fix_minuz(double d);
-static double bilinear(double x1, double y1, double x2, double y2, double x, double y, double z11, double z12, double z21, double z22)
+static double bilinear(double x1, double y1, double x2, double y2, double x,
+ double y, double z11, double z12, double z21,
+ double z22)
{
- double delta;
+ double delta;
- if (y1 == y2 && x1 == x2 ) return (z11);
- if (y1 == y2 && x1 != x2 ) return (z22*(x-x1)+z11*(x2-x))/(x2-x1);
- if (x1 == x2 && y1 != y2 ) return (z22*(y-y1)+z11*(y2-y))/(y2-y1);
+ if (y1 == y2 && x1 == x2)
+ return (z11);
+ if (y1 == y2 && x1 != x2)
+ return (z22 * (x - x1) + z11 * (x2 - x)) / (x2 - x1);
+ if (x1 == x2 && y1 != y2)
+ return (z22 * (y - y1) + z11 * (y2 - y)) / (y2 - y1);
- delta=(y2-y1)*(x2-x1);
+ delta = (y2 - y1) * (x2 - x1);
- return (z22*(y-y1)*(x-x1)+z12*(y2-y)*(x-x1)+z21*(y-y1)*(x2-x)+z11*(y2-y)*(x2-x))/delta;
+ return (z22 * (y - y1) * (x - x1) + z12 * (y2 - y) * (x - x1) +
+ z21 * (y - y1) * (x2 - x) + z11 * (y2 - y) * (x2 - x)) / delta;
}
/* return geoid separtion (MSL - WGS84) in meters, given a lat/lot in degrees */
+/* *INDENT-OFF* */
/*@ +charint @*/
double wgs84_separation(double lat, double lon)
{
@@ -55,65 +62,71 @@ double wgs84_separation(double lat, double lon)
/* 80N */ 3, 1, -2, -3, -3, -3, -1, 3, 1, 5, 9, 11, 19, 27, 31, 34, 33, 34, 33, 34, 28, 23, 17, 13, 9, 4, 4, 1, -2, -2, 0, 2, 3, 2, 1, 1, 3,
/* 90N */ 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13};
/*@ -charint @*/
- int ilat, ilon;
- int ilat1, ilat2, ilon1, ilon2;
+/* *INDENT-ON* */
+int ilat, ilon;
+int ilat1, ilat2, ilon1, ilon2;
+
+ilat = (int)floor((90. + lat) / 10);
+ilon = (int)floor((180. + lon) / 10);
- ilat=(int)floor(( 90.+lat)/10);
- ilon=(int)floor((180.+lon)/10);
-
/* sanity checks to prevent segfault on bad data */
- if ( ( ilat > 90 ) || ( ilat < -90 ) ) {
- return 0.0;
- }
- if ( ( ilon > 180 ) || ( ilon < -180 ) ) {
- return 0.0;
- }
-
- ilat1=ilat;
- ilon1=ilon;
- ilat2=(ilat < GEOID_ROW-1)? ilat+1:ilat;
- ilon2=(ilon < GEOID_COL-1)? ilon+1:ilon;
-
- return bilinear(
- ilon1*10.-180.,ilat1*10.-90.,
- ilon2*10.-180.,ilat2*10.-90.,
- lon, lat,
- (double)geoid_delta[ilon1+ilat1*GEOID_COL],
- (double)geoid_delta[ilon2+ilat1*GEOID_COL],
- (double)geoid_delta[ilon1+ilat2*GEOID_COL],
- (double)geoid_delta[ilon2+ilat2*GEOID_COL]
- );
+if ((ilat > 90) || (ilat < -90)) {
+ return 0.0;
+}
+if ((ilon > 180) || (ilon < -180)) {
+ return 0.0;
+}
+
+ilat1 = ilat;
+ilon1 = ilon;
+ilat2 = (ilat < GEOID_ROW - 1) ? ilat + 1 : ilat;
+ilon2 = (ilon < GEOID_COL - 1) ? ilon + 1 : ilon;
+
+return bilinear(ilon1 * 10. - 180., ilat1 * 10. - 90.,
+ ilon2 * 10. - 180., ilat2 * 10. - 90.,
+ lon, lat,
+ (double)geoid_delta[ilon1 + ilat1 * GEOID_COL],
+ (double)geoid_delta[ilon2 + ilat1 * GEOID_COL],
+ (double)geoid_delta[ilon1 + ilat2 * GEOID_COL],
+ (double)geoid_delta[ilon2 + ilat2 * GEOID_COL]
+ );
}
void ecef_to_wgs84fix(struct gps_fix_t *fix, double *separation,
- double x, double y, double z,
- double vx, double vy, double vz)
+ double x, double y, double z,
+ double vx, double vy, double vz)
/* fill in WGS84 position/velocity fields from ECEF coordinates */
{
- double lambda,phi,p,theta,n,h,vnorth,veast,heading;
- 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);
+ double lambda, phi, p, theta, n, h, vnorth, veast, heading;
+ 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);
/* geodetic location */
- lambda = atan2(y,x);
- /*@ -evalorder @*/
- p = sqrt(pow(x,2) + pow(y,2));
- theta = atan2(z*a,p*b);
- phi = atan2(z + e_2*b*pow(sin(theta),3),p - e2*a*pow(cos(theta),3));
- n = a / sqrt(1.0 - e2*pow(sin(phi),2));
+ lambda = atan2(y, x);
+ /*@ -evalorder @*/
+ p = sqrt(pow(x, 2) + pow(y, 2));
+ theta = atan2(z * a, p * b);
+ phi =
+ atan2(z + e_2 * b * pow(sin(theta), 3),
+ p - e2 * a * pow(cos(theta), 3));
+ n = a / sqrt(1.0 - e2 * pow(sin(phi), 2));
h = p / cos(phi) - n;
fix->latitude = phi * RAD_2_DEG;
fix->longitude = lambda * RAD_2_DEG;
*separation = wgs84_separation(fix->latitude, fix->longitude);
fix->altitude = h - *separation;
/* velocity computation */
- vnorth = -vx*sin(phi)*cos(lambda)-vy*sin(phi)*sin(lambda)+vz*cos(phi);
- veast = -vx*sin(lambda)+vy*cos(lambda);
- fix->climb = vx*cos(phi)*cos(lambda)+vy*cos(phi)*sin(lambda)+vz*sin(phi);
- fix->speed = sqrt(pow(vnorth,2) + pow(veast,2));
+ vnorth =
+ -vx * sin(phi) * cos(lambda) - vy * sin(phi) * sin(lambda) +
+ vz * cos(phi);
+ veast = -vx * sin(lambda) + vy * cos(lambda);
+ fix->climb =
+ vx * cos(phi) * cos(lambda) + vy * cos(phi) * sin(lambda) +
+ vz * sin(phi);
+ fix->speed = sqrt(pow(vnorth, 2) + pow(veast, 2));
heading = atan2(fix_minuz(veast), fix_minuz(vnorth));
/*@ +evalorder @*/
if (heading < 0)