gpsutils.c

来自「gpsd, a popular GPS daemon.」· C语言 代码 · 共 591 行 · 第 1/2 页

/* $Id: gpsutils.c 4652 2008-01-01 16:47:10Z ckuethe $ */
/* gpsutils.c -- code shared between low-level and high-level interfaces */
#include <sys/types.h>
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <stdarg.h>
#include <time.h>

#include "gpsd_config.h"
#include "gpsd.h"

#define MONTHSPERYEAR	12		/* months per calendar year */

void gps_clear_fix(/*@out@*/struct gps_fix_t *fixp)
/* stuff a fix structure with recognizable out-of-band values */
{
    fixp->time = NAN;
    fixp->mode = MODE_NOT_SEEN;
    fixp->latitude = fixp->longitude = NAN;
    fixp->track = NAN;
    fixp->speed = NAN;
    fixp->climb = NAN;
    fixp->altitude = NAN;
    fixp->ept = NAN;
    fixp->eph = NAN;
    fixp->epv = NAN;
    fixp->epd = NAN;
    fixp->eps = NAN;
    fixp->epc = NAN;
}

unsigned int gps_valid_fields(/*@in@*/struct gps_fix_t *fixp)
{
    unsigned int valid = 0;

    if (isnan(fixp->time) == 0)
	valid |= TIME_SET;
    if (fixp->mode != MODE_NOT_SEEN)
	valid |= MODE_SET;
    if (isnan(fixp->latitude) == 0 && isnan(fixp->longitude) == 0)
	valid |= LATLON_SET;
    if (isnan(fixp->altitude) == 0)
	valid |= ALTITUDE_SET;
    if (isnan(fixp->track) == 0)
	valid |= TRACK_SET;
    if (isnan(fixp->speed) == 0)
	valid |= SPEED_SET;
    if (isnan(fixp->climb) == 0)
	valid |= CLIMB_SET;
    if (isnan(fixp->ept) == 0)
	valid |= TIMERR_SET;
    if (isnan(fixp->eph) == 0)
	valid |= HERR_SET;
    if (isnan(fixp->epv) == 0)
	valid |= VERR_SET;
    if (isnan(fixp->epd) == 0)
	valid |= TRACKERR_SET;
    if (isnan(fixp->eps) == 0)
	valid |= SPEEDERR_SET;
    if (isnan(fixp->epc) == 0)
	valid |= CLIMBERR_SET;
    return valid;
}

char *gps_show_transfer(int transfer)
{
/*@ -statictrans @*/
    static char showbuf[100];
    showbuf[0] = '\0';
    if ((transfer & TIME_SET)!=0)
	(void)strlcat(showbuf, "time,", sizeof(showbuf));
    if ((transfer & LATLON_SET)!=0)
	(void)strlcat(showbuf, "latlon,", sizeof(showbuf));
    if ((transfer & MODE_SET)!=0)
	(void)strlcat(showbuf, "mode,", sizeof(showbuf));
    if ((transfer & ALTITUDE_SET)!=0)
	(void)strlcat(showbuf, "altitude,", sizeof(showbuf));
    if ((transfer & TRACK_SET)!=0)
	(void)strlcat(showbuf, "track,", sizeof(showbuf));
    if ((transfer & SPEED_SET)!=0)
	(void)strlcat(showbuf, "speed,", sizeof(showbuf));
    if ((transfer & CLIMB_SET)!=0)
	(void)strlcat(showbuf, "climb,", sizeof(showbuf));
    if ((transfer & TIMERR_SET)!=0)
	(void)strlcat(showbuf, "timerr,", sizeof(showbuf));
    if ((transfer & HERR_SET)!=0)
	(void)strlcat(showbuf, "herr,", sizeof(showbuf));
    if ((transfer & VERR_SET)!=0)
	(void)strlcat(showbuf, "verr,", sizeof(showbuf));
    if ((transfer & SPEEDERR_SET)!=0)
	(void)strlcat(showbuf, "speederr,", sizeof(showbuf));
    if ((transfer & CLIMBERR_SET)!=0)
	(void)strlcat(showbuf, "climberr,", sizeof(showbuf));
    if (strlen(showbuf)>0)
	showbuf[strlen(showbuf)-1] = '\0';
    return showbuf;
/*@ +statictrans @*/
}

void gps_merge_fix(/*@ out @*/struct gps_fix_t *to,
		   gps_mask_t transfer,
		   /*@ in @*/struct gps_fix_t *from)
/* merge new data into an old fix */
{
    if ((NULL == to) || (NULL == from))
	return;
    if ((transfer & TIME_SET)!=0)
	to->time = from->time;
    if ((transfer & LATLON_SET)!=0) {
	to->latitude = from->latitude;
	to->longitude = from->longitude;
    }
    if ((transfer & MODE_SET)!=0)
	to->mode = from->mode;
    if ((transfer & ALTITUDE_SET)!=0)
	to->altitude = from->altitude;
    if ((transfer & TRACK_SET)!=0)
	to->track = from->track;
    if ((transfer & SPEED_SET)!=0)
	to->speed = from->speed;
    if ((transfer & CLIMB_SET)!=0)
	to->climb = from->climb;
    if ((transfer & TIMERR_SET)!=0)
	to->ept = from->ept;
    if ((transfer & HERR_SET)!=0)
	to->eph = from->eph;
    if ((transfer & VERR_SET)!=0)
	to->epv = from->epv;
    if ((transfer & SPEEDERR_SET)!=0)
	to->eps = from->eps;
    if ((transfer & CLIMBERR_SET)!=0)
	to->epc = from->epc;
}

double timestamp(void) 
{
    struct timeval tv; 
    (void)gettimeofday(&tv, NULL); 
    /*@i1@*/return(tv.tv_sec + tv.tv_usec*1e-6);
}

time_t mkgmtime(register struct tm *t)
/* struct tm to seconds since Unix epoch */
{
    register int year;
    register time_t result;
    static const int cumdays[MONTHSPERYEAR] =
    {0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334};

    /*@ +matchanyintegral @*/
    year = 1900 + t->tm_year + t->tm_mon / MONTHSPERYEAR;
    result = (year - 1970) * 365 + cumdays[t->tm_mon % MONTHSPERYEAR];
    result += (year - 1968) / 4;
    result -= (year - 1900) / 100;
    result += (year - 1600) / 400;
    if ((year % 4) == 0 && ((year % 100) != 0 || (year % 400) == 0) &&
	(t->tm_mon % MONTHSPERYEAR) < 2)
	     result--;
    result += t->tm_mday - 1;
    result *= 24;
    result += t->tm_hour;
    result *= 60;
    result += t->tm_min;
    result *= 60;
    result += t->tm_sec;
    /*@ -matchanyintegral @*/
    return (result);
}

double iso8601_to_unix(/*@in@*/char *isotime)
/* ISO8601 UTC to Unix UTC */
{
    char *dp = NULL;
    double usec;
    struct tm tm;

    /*@i1@*/dp = strptime(isotime, "%Y-%m-%dT%H:%M:%S", &tm);
    if (*dp == '.')
	usec = strtod(dp, NULL);
    else
	usec = 0;
    return (double)mkgmtime(&tm) + usec;
}

/*@observer@*/char *unix_to_iso8601(double fixtime, /*@ out @*/char isotime[], size_t len)
/* Unix UTC time to ISO8601, no timezone adjustment */
/* example: 2007-12-11T23:38:51.0Z */
{
    struct tm when;
    double integral, fractional;
    time_t intfixtime;
    char timestr[30];
    char fractstr[10];

    fractional = modf(fixtime, &integral);
    intfixtime = (time_t)integral;
    (void)gmtime_r(&intfixtime, &when);

    (void)strftime(timestr, sizeof(timestr), "%Y-%m-%dT%H:%M:%S", &when);
    (void)snprintf(fractstr, sizeof(fractstr), "%.1f", fractional);
    /* add fractional part, ignore leading 0; "0.2" -> ".2" */ 
    (void)snprintf(isotime, len, "%s%sZ", timestr, fractstr+1);
    return isotime;
}

/*
 * The 'week' part of GPS dates are specified in weeks since 0000 on 06 
 * January 1980, with a rollover at 1024.  At time of writing the last 
 * rollover happened at 0000 22 August 1999.  Time-of-week is in seconds.
 *
 * This code copes with both conventional GPS weeks and the "extended"
 * 15-or-16-bit version with no wraparound that appears in Zodiac
 * chips and is supposed to appear in the Geodetic Navigation
 * Information (0x29) packet of SiRF chips.  Some SiRF firmware versions
 * (notably 231) actually ship the wrapped 10-bit week, despite what
 * the protocol reference claims.
 *
 * Note: This time will need to be corrected for leap seconds.
 */
#define GPS_EPOCH	315964800		/* GPS epoch in Unix time */
#define SECS_PER_WEEK	(60*60*24*7)		/* seconds per week */
#define GPS_ROLLOVER	(1024*SECS_PER_WEEK)	/* rollover period */

double gpstime_to_unix(int week, double tow)
{
    double fixtime;

    if (week >= 1024)
	fixtime = GPS_EPOCH + (week * SECS_PER_WEEK) + tow;
    else {
	time_t now, last_rollover;
	(void)time(&now);
	last_rollover = GPS_EPOCH+((now-GPS_EPOCH)/GPS_ROLLOVER)*GPS_ROLLOVER;
	/*@i@*/fixtime = last_rollover + (week * SECS_PER_WEEK) + tow;
    }
    return fixtime;
}

void unix_to_gpstime(double unixtime, /*@out@*/int *week, /*@out@*/double *tow)
{
    unixtime -= GPS_EPOCH;
    *week = (int)(unixtime / SECS_PER_WEEK);
    *tow = fmod(unixtime, SECS_PER_WEEK);
}

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