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