1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
|
/* gpsutils.c -- code shared between low-level and high-level interfaces */
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <stdarg.h>
#define __USE_XOPEN
#include <time.h>
#include "gpsd.h"
double timestamp(void)
{
struct timeval tv;
gettimeofday(&tv, NULL);
return(tv.tv_sec + tv.tv_usec/1e6);
}
time_t mkgmtime(register struct tm *t)
/* struct tm to seconds since Unix epoch */
{
register unsigned short year;
register time_t result;
static const int cumdays[12] =
{0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334};
year = 1900 + t->tm_year + t->tm_mon / 12;
result = (year - 1970) * 365 + cumdays[t->tm_mon % 12];
result += (year - 1968) / 4;
result -= (year - 1900) / 100;
result += (year - 1600) / 400;
result += t->tm_mday - 1;
result *= 24;
result += t->tm_hour;
result *= 60;
result += t->tm_min;
result *= 60;
result += t->tm_sec;
return (result);
}
double iso8601_to_unix(char *isotime)
/* ISO8601 UTC to Unix UTC */
{
char *dp = NULL;
double usec;
struct tm tm;
dp = strptime(isotime, "%Y-%m-%dT%H:%M:%S", &tm);
if (*dp == '.')
usec = strtod(dp, NULL);
else
usec = 0;
return mkgmtime(&tm) + usec;
}
char *unix_to_iso8601(double fixtime, char *isotime)
/* Unix UTC time to ISO8601, no timezone adjustment */
{
struct tm when;
double integral, fractional;
time_t intfixtime;
int slen;
fractional = modf(fixtime, &integral);
intfixtime = (time_t)integral;
gmtime_r(&intfixtime, &when);
strftime(isotime, 28, "%Y-%m-%dT%H:%M:%S", &when);
slen = strlen(isotime);
sprintf(isotime + slen, "%.1f", fractional);
memcpy(isotime+slen, isotime+slen+1, strlen(isotime+slen+1));
strcat(isotime, "Z");
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 apperas 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.
* That's what the offset argument is for.
*/
#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, int offset)
{
double fixtime;
if (week >= GPS_ROLLOVER)
fixtime = GPS_EPOCH + (week * SECS_PER_WEEK) + tow;
else {
time_t now, last_rollover;
time(&now);
last_rollover = GPS_EPOCH+((now-GPS_EPOCH)/GPS_ROLLOVER)*GPS_ROLLOVER;
fixtime = last_rollover + (week * SECS_PER_WEEK) + tow;
}
return fixtime + offset;
}
#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). Thus we constrain it here.
if (abs(a) > 1)
a = 1;
else if (a < -1)
a = -1;
return CalcRad((lat1+lat2) / 2) * acos(a);
}
|
