int main(int argc, char **argv) { double lat, lon; if (argc != 3) { fprintf(stderr, "Usage: %s lat lon\n", argv[0]); return 1; } lat = safe_atof(argv[1]); lon = safe_atof(argv[2]); if (lat > 90. || lat < -90.) { fprintf(stderr, " -90 <= lat=%s(%.f) <= 90 ?\n", argv[1], lat); return 1; } if (lon > 180. || lat < -180.) { fprintf(stderr, " -180 <= lon=%s(%.f) <= 180 ?\n", argv[2], lon); return 1; } printf(" lat= %f lon= %f geoid correction= %f\n", lat, lon, wgs84_separation(lat, lon)); return 0; }
static void decode_ecef(double x, double y, double z, double vx, double vy, double vz) { 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; /* splint pacification */ assert(mid2win!=NULL); 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; h -= wgs84_separation((double)(RAD_2_DEG * phi), (double)(RAD_2_DEG * lambda)); vnorth = -vx * sin(phi) * cos(lambda) - vy * sin(phi) * sin(lambda) + vz * cos(phi); veast = -vx * sin(lambda) + vy * cos(lambda); vup = vx * cos(phi) * cos(lambda) + vy * cos(phi) * sin(lambda) + vz * sin(phi); speed = sqrt(pow(vnorth, 2) + pow(veast, 2)); heading = atan2(veast, vnorth); /*@ +evalorder @*/ if (heading < 0) heading += 2 * GPS_PI; /* North and East position fields */ (void)wattrset(mid2win, A_UNDERLINE); (void)wmove(mid2win, 1, 40); (void)wprintw(mid2win, "%9.5f %9.5f %9d", (double)(RAD_2_DEG * phi), (double)(RAD_2_DEG * lambda), (int)h); (void)mvwaddch(mid2win, 1, 49, ACS_DEGREE); (void)mvwaddch(mid2win, 1, 59, ACS_DEGREE); /* North and East velocity fields */ (void)wmove(mid2win, 2, 40); (void)wprintw(mid2win, "%9.1f %9.1f %9.1f", vnorth, veast, vup); /* heading and speed fields */ (void)wmove(mid2win, 3, 54); (void)wprintw(mid2win, "%5.1f %9.1f", (double)(RAD_2_DEG * heading), speed); (void)mvwaddch(mid2win, 3, 59, ACS_DEGREE); (void)wattrset(mid2win, A_NORMAL); }
void ecef_to_wgs84fix(struct gps_fix_t *fix, double *separation, 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); /* 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)); 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)); heading = atan2(fix_minuz(veast), fix_minuz(vnorth)); /*@ +evalorder @*/ if (heading < 0) heading += 2 * GPS_PI; fix->track = heading * RAD_2_DEG; }
/* * Decode the navigation solution message */ static gps_mask_t oncore_msg_navsol(struct gps_device_t *session, unsigned char *buf, size_t data_len) { gps_mask_t mask; unsigned char flags; double lat, lon, alt; float speed, track, dop; unsigned int i, j, st, nsv; int Bbused; struct tm unpacked_date; if (data_len != 76) return 0; mask = ONLINE_SET; gpsd_log(&session->context->errout, LOG_DATA, "oncore NAVSOL - navigation data\n"); flags = (unsigned char)getub(buf, 72); /*@ -predboolothers @*/ if (flags & 0x20) { session->gpsdata.status = STATUS_FIX; session->newdata.mode = MODE_3D; } else if (flags & 0x10) { session->gpsdata.status = STATUS_FIX; session->newdata.mode = MODE_2D; } else { gpsd_log(&session->context->errout, LOG_WARN, "oncore NAVSOL no fix - flags 0x%02x\n", flags); session->newdata.mode = MODE_NO_FIX; session->gpsdata.status = STATUS_NO_FIX; } mask |= MODE_SET; /*@ +predboolothers @*/ /* Unless we have seen non-zero utc offset data, the time is GPS time * and not UTC time. Do not use it. */ if (session->context->leap_seconds) { unsigned int nsec; unpacked_date.tm_mon = (int)getub(buf, 4) - 1; unpacked_date.tm_mday = (int)getub(buf, 5); unpacked_date.tm_year = (int)getbeu16(buf, 6) - 1900; unpacked_date.tm_hour = (int)getub(buf, 8); unpacked_date.tm_min = (int)getub(buf, 9); unpacked_date.tm_sec = (int)getub(buf, 10); unpacked_date.tm_isdst = 0; #ifdef S_SPLINT_S unpacked_date.tm_wday = unpacked_date.tm_yday = 0; #endif /* S_SPLINT_S */ nsec = (uint) getbeu32(buf, 11); /*@ -unrecog */ session->newdata.time = (timestamp_t)mkgmtime(&unpacked_date) + nsec * 1e-9; /*@ +unrecog */ mask |= TIME_SET; gpsd_log(&session->context->errout, LOG_DATA, "oncore NAVSOL - time: %04d-%02d-%02d %02d:%02d:%02d.%09d\n", unpacked_date.tm_year + 1900, unpacked_date.tm_mon + 1, unpacked_date.tm_mday, unpacked_date.tm_hour, unpacked_date.tm_min, unpacked_date.tm_sec, nsec); } /*@-type@*/ lat = getbes32(buf, 15) / 3600000.0f; lon = getbes32(buf, 19) / 3600000.0f; alt = getbes32(buf, 23) / 100.0f; speed = getbeu16(buf, 31) / 100.0f; track = getbeu16(buf, 33) / 10.0f; dop = getbeu16(buf, 35) / 10.0f; /*@+type@*/ gpsd_log(&session->context->errout, LOG_DATA, "oncore NAVSOL - %lf %lf %.2lfm-%.2lfm | %.2fm/s %.1fdeg dop=%.1f\n", lat, lon, alt, wgs84_separation(lat, lon), speed, track, (float)dop); session->newdata.latitude = lat; session->newdata.longitude = lon; session->gpsdata.separation = wgs84_separation(session->newdata.latitude, session->newdata.longitude); session->newdata.altitude = alt - session->gpsdata.separation; session->newdata.speed = speed; session->newdata.track = track; mask |= LATLON_SET | ALTITUDE_SET | SPEED_SET | TRACK_SET; gpsd_zero_satellites(&session->gpsdata); /* Merge the satellite information from the Bb message. */ Bbused = 0; nsv = 0; for (i = st = 0; i < 8; i++) { int sv, mode, sn, status; unsigned int off; off = 40 + 4 * i; sv = (int)getub(buf, off); mode = (int)getub(buf, off + 1); sn = (int)getub(buf, off + 2); status = (int)getub(buf, off + 3); gpsd_log(&session->context->errout, LOG_DATA, "%2d %2d %2d %3d %02x\n", i, sv, mode, sn, status); if (sn) { session->gpsdata.skyview[st].PRN = (short)sv; session->gpsdata.skyview[st].ss = (double)sn; for (j = 0; (int)j < session->driver.oncore.visible; j++) if (session->driver.oncore.PRN[j] == sv) { session->gpsdata.skyview[st].elevation = (short)session->driver.oncore.elevation[j]; session->gpsdata.skyview[st].azimuth = (short)session->driver.oncore.azimuth[j]; Bbused |= 1 << j; break; } /* bit 7 of the status word: sat used for position */ session->gpsdata.skyview[st].used = false; if (status & 0x80) { session->gpsdata.skyview[st].used = true; nsv++; } /* bit 2 of the status word: using for time solution */ if (status & 0x02) mask |= PPSTIME_IS; /* * The PPSTIME_IS mask bit exists distinctly from TIME_SET exactly * so an OnCore running in time-service mode (and other GPS clocks) * can signal that it's returning time even though no position fixes * have been available. */ st++; } } for (j = 0; (int)j < session->driver.oncore.visible; j++) /*@ -boolops @*/ if (!(Bbused & (1 << j))) { session->gpsdata.skyview[st].PRN = (short)session->driver.oncore.PRN[j]; session->gpsdata.skyview[st].elevation = (short)session->driver.oncore.elevation[j]; session->gpsdata.skyview[st].azimuth = (short)session->driver.oncore.azimuth[j]; st++; } /*@ +boolops @*/ session->gpsdata.skyview_time = session->newdata.time; session->gpsdata.satellites_used = (int)nsv; session->gpsdata.satellites_visible = (int)st; mask |= SATELLITE_SET | USED_IS; /* Some messages can only be polled. As they are not so * important, would be enough to poll e.g. one message per cycle. */ (void)oncore_control_send(session, (char *)pollAs, sizeof(pollAs)); (void)oncore_control_send(session, (char *)pollAt, sizeof(pollAt)); (void)oncore_control_send(session, (char *)pollAy, sizeof(pollAy)); (void)oncore_control_send(session, (char *)pollBo, sizeof(pollBo)); (void)oncore_control_send(session, (char *)pollEn, sizeof(pollEn)); gpsd_log(&session->context->errout, LOG_DATA, "NAVSOL: time=%.2f lat=%.2f lon=%.2f alt=%.2f speed=%.2f track=%.2f mode=%d status=%d visible=%d used=%d\n", session->newdata.time, session->newdata.latitude, session->newdata.longitude, session->newdata.altitude, session->newdata.speed, session->newdata.track, session->newdata.mode, session->gpsdata.status, session->gpsdata.satellites_used, session->gpsdata.satellites_visible); return mask; }