User | Revision | Line number | New contents of line |
joosttromp |
0:62fa44dd600b
|
1
|
/*
|
joosttromp |
0:62fa44dd600b
|
2
|
Copyright (c) 2010 Andy Kirkham
|
joosttromp |
0:62fa44dd600b
|
3
|
|
joosttromp |
0:62fa44dd600b
|
4
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
joosttromp |
0:62fa44dd600b
|
5
|
of this software and associated documentation files (the "Software"), to deal
|
joosttromp |
0:62fa44dd600b
|
6
|
in the Software without restriction, including without limitation the rights
|
joosttromp |
0:62fa44dd600b
|
7
|
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
joosttromp |
0:62fa44dd600b
|
8
|
copies of the Software, and to permit persons to whom the Software is
|
joosttromp |
0:62fa44dd600b
|
9
|
furnished to do so, subject to the following conditions:
|
joosttromp |
0:62fa44dd600b
|
10
|
|
joosttromp |
0:62fa44dd600b
|
11
|
The above copyright notice and this permission notice shall be included in
|
joosttromp |
0:62fa44dd600b
|
12
|
all copies or substantial portions of the Software.
|
joosttromp |
0:62fa44dd600b
|
13
|
|
joosttromp |
0:62fa44dd600b
|
14
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
joosttromp |
0:62fa44dd600b
|
15
|
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
joosttromp |
0:62fa44dd600b
|
16
|
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
joosttromp |
0:62fa44dd600b
|
17
|
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
joosttromp |
0:62fa44dd600b
|
18
|
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
joosttromp |
0:62fa44dd600b
|
19
|
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
joosttromp |
0:62fa44dd600b
|
20
|
THE SOFTWARE.
|
joosttromp |
0:62fa44dd600b
|
21
|
*/
|
joosttromp |
0:62fa44dd600b
|
22
|
|
joosttromp |
0:62fa44dd600b
|
23
|
#include "GPS_Time.h"
|
joosttromp |
0:62fa44dd600b
|
24
|
|
joosttromp |
0:62fa44dd600b
|
25
|
//GPS_Time *t = gps.timeNow();
|
joosttromp |
0:62fa44dd600b
|
26
|
//pc.printf("%02d:%02d:%02d.%i %02d/%02d/%04d\r\n\n", t->hour, t->minute, t->second, t->hundreths, t->day, t->month, t->year);
|
joosttromp |
0:62fa44dd600b
|
27
|
//delete(t);
|
joosttromp |
0:62fa44dd600b
|
28
|
|
joosttromp |
0:62fa44dd600b
|
29
|
GPS_Time::GPS_Time()
|
joosttromp |
0:62fa44dd600b
|
30
|
{
|
joosttromp |
0:62fa44dd600b
|
31
|
year = 2000;
|
joosttromp |
0:62fa44dd600b
|
32
|
month = 1;
|
joosttromp |
0:62fa44dd600b
|
33
|
day = 1;
|
joosttromp |
0:62fa44dd600b
|
34
|
hour = 0;
|
joosttromp |
0:62fa44dd600b
|
35
|
minute = 0;
|
joosttromp |
0:62fa44dd600b
|
36
|
second = 0;
|
joosttromp |
0:62fa44dd600b
|
37
|
tenths = 0;
|
joosttromp |
0:62fa44dd600b
|
38
|
hundreths = 0;
|
joosttromp |
0:62fa44dd600b
|
39
|
status = 'V';
|
joosttromp |
0:62fa44dd600b
|
40
|
velocity = 0;
|
joosttromp |
0:62fa44dd600b
|
41
|
track = 0;
|
joosttromp |
0:62fa44dd600b
|
42
|
magvar_dir = 'W';
|
joosttromp |
0:62fa44dd600b
|
43
|
magvar = 0;
|
joosttromp |
0:62fa44dd600b
|
44
|
}
|
joosttromp |
0:62fa44dd600b
|
45
|
|
joosttromp |
0:62fa44dd600b
|
46
|
time_t
|
joosttromp |
0:62fa44dd600b
|
47
|
GPS_Time::to_C_tm(bool set)
|
joosttromp |
0:62fa44dd600b
|
48
|
{
|
joosttromp |
0:62fa44dd600b
|
49
|
GPS_Time t;
|
joosttromp |
0:62fa44dd600b
|
50
|
tm ct;
|
joosttromp |
0:62fa44dd600b
|
51
|
time_t q;
|
joosttromp |
0:62fa44dd600b
|
52
|
|
joosttromp |
0:62fa44dd600b
|
53
|
timeNow(&t);
|
joosttromp |
0:62fa44dd600b
|
54
|
ct.tm_sec = t.second;
|
joosttromp |
0:62fa44dd600b
|
55
|
ct.tm_min = t.minute;
|
joosttromp |
0:62fa44dd600b
|
56
|
ct.tm_hour = t.hour;
|
joosttromp |
0:62fa44dd600b
|
57
|
ct.tm_mday = t.day;
|
joosttromp |
0:62fa44dd600b
|
58
|
ct.tm_mon = t.month - 1;
|
joosttromp |
0:62fa44dd600b
|
59
|
ct.tm_year = t.year - 1900;
|
joosttromp |
0:62fa44dd600b
|
60
|
ct.tm_isdst = 0; // GPS has no understanding of DST.
|
joosttromp |
0:62fa44dd600b
|
61
|
|
joosttromp |
0:62fa44dd600b
|
62
|
q = mktime(&ct);
|
joosttromp |
0:62fa44dd600b
|
63
|
if (set) {
|
joosttromp |
0:62fa44dd600b
|
64
|
set_time(q);
|
joosttromp |
0:62fa44dd600b
|
65
|
}
|
joosttromp |
0:62fa44dd600b
|
66
|
return q;
|
joosttromp |
0:62fa44dd600b
|
67
|
}
|
joosttromp |
0:62fa44dd600b
|
68
|
|
joosttromp |
0:62fa44dd600b
|
69
|
GPS_Time *
|
joosttromp |
0:62fa44dd600b
|
70
|
GPS_Time::timeNow(GPS_Time *n)
|
joosttromp |
0:62fa44dd600b
|
71
|
{
|
joosttromp |
0:62fa44dd600b
|
72
|
if (n == NULL) n = new GPS_Time;
|
joosttromp |
0:62fa44dd600b
|
73
|
|
joosttromp |
0:62fa44dd600b
|
74
|
do {
|
joosttromp |
0:62fa44dd600b
|
75
|
memcpy(n, this, sizeof(GPS_Time));
|
joosttromp |
0:62fa44dd600b
|
76
|
}
|
joosttromp |
0:62fa44dd600b
|
77
|
while (memcmp(n, this, sizeof(GPS_Time)));
|
joosttromp |
0:62fa44dd600b
|
78
|
return n;
|
joosttromp |
0:62fa44dd600b
|
79
|
}
|
joosttromp |
0:62fa44dd600b
|
80
|
|
joosttromp |
0:62fa44dd600b
|
81
|
void
|
joosttromp |
0:62fa44dd600b
|
82
|
GPS_Time::operator++()
|
joosttromp |
0:62fa44dd600b
|
83
|
{
|
joosttromp |
0:62fa44dd600b
|
84
|
hundreths++;
|
joosttromp |
0:62fa44dd600b
|
85
|
if (hundreths == 10) {
|
joosttromp |
0:62fa44dd600b
|
86
|
hundreths = 0;
|
joosttromp |
0:62fa44dd600b
|
87
|
tenths++;
|
joosttromp |
0:62fa44dd600b
|
88
|
if (tenths == 10) {
|
joosttromp |
0:62fa44dd600b
|
89
|
tenths = hundreths = 0;
|
joosttromp |
0:62fa44dd600b
|
90
|
}
|
joosttromp |
0:62fa44dd600b
|
91
|
}
|
joosttromp |
0:62fa44dd600b
|
92
|
}
|
joosttromp |
0:62fa44dd600b
|
93
|
|
joosttromp |
0:62fa44dd600b
|
94
|
void
|
joosttromp |
0:62fa44dd600b
|
95
|
GPS_Time::operator++(int)
|
joosttromp |
0:62fa44dd600b
|
96
|
{
|
joosttromp |
0:62fa44dd600b
|
97
|
const int days[12] = { 31,28,31,30,31,30,31,31,30,31,30,31 };
|
joosttromp |
0:62fa44dd600b
|
98
|
bool dateInc = false;
|
joosttromp |
0:62fa44dd600b
|
99
|
|
joosttromp |
0:62fa44dd600b
|
100
|
tenths = hundreths = 0;
|
joosttromp |
0:62fa44dd600b
|
101
|
second++;
|
joosttromp |
0:62fa44dd600b
|
102
|
|
joosttromp |
0:62fa44dd600b
|
103
|
if (second == 60) {
|
joosttromp |
0:62fa44dd600b
|
104
|
second = 0;
|
joosttromp |
0:62fa44dd600b
|
105
|
minute++;
|
joosttromp |
0:62fa44dd600b
|
106
|
if (minute == 60) {
|
joosttromp |
0:62fa44dd600b
|
107
|
minute = 0;
|
joosttromp |
0:62fa44dd600b
|
108
|
hour++;
|
joosttromp |
0:62fa44dd600b
|
109
|
if (hour == 24) {
|
joosttromp |
0:62fa44dd600b
|
110
|
hour = 0;
|
joosttromp |
0:62fa44dd600b
|
111
|
dateInc = true;
|
joosttromp |
0:62fa44dd600b
|
112
|
}
|
joosttromp |
0:62fa44dd600b
|
113
|
}
|
joosttromp |
0:62fa44dd600b
|
114
|
}
|
joosttromp |
0:62fa44dd600b
|
115
|
|
joosttromp |
0:62fa44dd600b
|
116
|
if (dateInc) {
|
joosttromp |
0:62fa44dd600b
|
117
|
/* Handle February leap year. */
|
joosttromp |
0:62fa44dd600b
|
118
|
int leap_year = ((year % 4 == 0 && year % 100 != 0) || year % 400 == 0) ? 1 : 0;
|
joosttromp |
0:62fa44dd600b
|
119
|
int days_this_month = days[month - 1];
|
joosttromp |
0:62fa44dd600b
|
120
|
if (month == 2 && leap_year) days_this_month++;
|
joosttromp |
0:62fa44dd600b
|
121
|
day++;
|
joosttromp |
0:62fa44dd600b
|
122
|
if (day > days_this_month) {
|
joosttromp |
0:62fa44dd600b
|
123
|
day = 1;
|
joosttromp |
0:62fa44dd600b
|
124
|
month++;
|
joosttromp |
0:62fa44dd600b
|
125
|
if (month == 13) {
|
joosttromp |
0:62fa44dd600b
|
126
|
year++;
|
joosttromp |
0:62fa44dd600b
|
127
|
}
|
joosttromp |
0:62fa44dd600b
|
128
|
}
|
joosttromp |
0:62fa44dd600b
|
129
|
}
|
joosttromp |
0:62fa44dd600b
|
130
|
}
|
joosttromp |
0:62fa44dd600b
|
131
|
|
joosttromp |
0:62fa44dd600b
|
132
|
// $GPRMC,112709.735,A,5611.5340,N,00302.0306,W,000.0,307.0,150411,,,A*70
|
joosttromp |
0:62fa44dd600b
|
133
|
void
|
joosttromp |
0:62fa44dd600b
|
134
|
GPS_Time::nmea_rmc(char *s)
|
joosttromp |
0:62fa44dd600b
|
135
|
{
|
joosttromp |
0:62fa44dd600b
|
136
|
char *token;
|
joosttromp |
0:62fa44dd600b
|
137
|
int token_counter = 0;
|
joosttromp |
0:62fa44dd600b
|
138
|
char *time = (char *)NULL;
|
joosttromp |
0:62fa44dd600b
|
139
|
char *date = (char *)NULL;
|
joosttromp |
0:62fa44dd600b
|
140
|
char *stat = (char *)NULL;
|
joosttromp |
0:62fa44dd600b
|
141
|
char *vel = (char *)NULL;
|
joosttromp |
0:62fa44dd600b
|
142
|
char *trk = (char *)NULL;
|
joosttromp |
0:62fa44dd600b
|
143
|
char *magv = (char *)NULL;
|
joosttromp |
0:62fa44dd600b
|
144
|
char *magd = (char *)NULL;
|
joosttromp |
0:62fa44dd600b
|
145
|
|
joosttromp |
0:62fa44dd600b
|
146
|
token = strtok(s, ",");
|
joosttromp |
0:62fa44dd600b
|
147
|
while (token) {
|
joosttromp |
0:62fa44dd600b
|
148
|
switch (token_counter) {
|
joosttromp |
0:62fa44dd600b
|
149
|
case 9: date = token; break;
|
joosttromp |
0:62fa44dd600b
|
150
|
case 1: time = token; break;
|
joosttromp |
0:62fa44dd600b
|
151
|
case 2: stat = token; break;
|
joosttromp |
0:62fa44dd600b
|
152
|
case 7: vel = token; break;
|
joosttromp |
0:62fa44dd600b
|
153
|
case 8: trk = token; break;
|
joosttromp |
0:62fa44dd600b
|
154
|
case 10: magv = token; break;
|
joosttromp |
0:62fa44dd600b
|
155
|
case 11: magd = token; break;
|
joosttromp |
0:62fa44dd600b
|
156
|
}
|
joosttromp |
0:62fa44dd600b
|
157
|
token = strtok((char *)NULL, ",");
|
joosttromp |
0:62fa44dd600b
|
158
|
token_counter++;
|
joosttromp |
0:62fa44dd600b
|
159
|
}
|
joosttromp |
0:62fa44dd600b
|
160
|
|
joosttromp |
0:62fa44dd600b
|
161
|
if (stat && date && time) {
|
joosttromp |
0:62fa44dd600b
|
162
|
hour = (char)((time[0] - '0') * 10) + (time[1] - '0');
|
joosttromp |
0:62fa44dd600b
|
163
|
minute = (char)((time[2] - '0') * 10) + (time[3] - '0');
|
joosttromp |
0:62fa44dd600b
|
164
|
second = (char)((time[4] - '0') * 10) + (time[5] - '0');
|
joosttromp |
0:62fa44dd600b
|
165
|
day = (char)((date[0] - '0') * 10) + (date[1] - '0');
|
joosttromp |
0:62fa44dd600b
|
166
|
month = (char)((date[2] - '0') * 10) + (date[3] - '0');
|
joosttromp |
0:62fa44dd600b
|
167
|
year = (int)((date[4] - '0') * 10) + (date[5] - '0') + 2000;
|
joosttromp |
0:62fa44dd600b
|
168
|
status = stat[0];
|
joosttromp |
0:62fa44dd600b
|
169
|
velocity = atof(vel);
|
joosttromp |
0:62fa44dd600b
|
170
|
track = atof(trk);
|
joosttromp |
0:62fa44dd600b
|
171
|
magvar = atof(magv);
|
joosttromp |
0:62fa44dd600b
|
172
|
magvar_dir = magd[0];
|
joosttromp |
0:62fa44dd600b
|
173
|
}
|
joosttromp |
0:62fa44dd600b
|
174
|
}
|
joosttromp |
0:62fa44dd600b
|
175
|
|
joosttromp |
0:62fa44dd600b
|
176
|
double
|
joosttromp |
0:62fa44dd600b
|
177
|
GPS_Time::julian_day_number(GPS_Time *t) {
|
joosttromp |
0:62fa44dd600b
|
178
|
double wikipedia_jdn = (double)(1461 * ((int)t->year + 4800 + ((int)t->month - 14) / 12)) / 4 + (367 * ((int)t->month - 2 - 12 * (((int)t->month - 14) / 12))) / 12 - (3 * (((int)t->year + 4900 + ((int)t->month - 14) / 12 ) / 100)) / 4 + (int)t->day - 32075;
|
joosttromp |
0:62fa44dd600b
|
179
|
return wikipedia_jdn;
|
joosttromp |
0:62fa44dd600b
|
180
|
}
|
joosttromp |
0:62fa44dd600b
|
181
|
|
joosttromp |
0:62fa44dd600b
|
182
|
double
|
joosttromp |
0:62fa44dd600b
|
183
|
GPS_Time::julian_date(GPS_Time *t) {
|
joosttromp |
0:62fa44dd600b
|
184
|
double hour, minute, second, jd;
|
joosttromp |
0:62fa44dd600b
|
185
|
hour = (double)t->hour;
|
joosttromp |
0:62fa44dd600b
|
186
|
minute = (double)t->minute;
|
joosttromp |
0:62fa44dd600b
|
187
|
second = (double)t->second + ((double)t->tenths / 10.) + ((double)t->hundreths / 100.);
|
joosttromp |
0:62fa44dd600b
|
188
|
|
joosttromp |
0:62fa44dd600b
|
189
|
jd = julian_day_number(t) - 0.5 +
|
joosttromp |
0:62fa44dd600b
|
190
|
((hour - 12.) / 24.) +
|
joosttromp |
0:62fa44dd600b
|
191
|
(minute / 1440.) +
|
joosttromp |
0:62fa44dd600b
|
192
|
(second / 86400.);
|
joosttromp |
0:62fa44dd600b
|
193
|
|
joosttromp |
0:62fa44dd600b
|
194
|
return jd;
|
joosttromp |
0:62fa44dd600b
|
195
|
}
|
joosttromp |
0:62fa44dd600b
|
196
|
|
joosttromp |
0:62fa44dd600b
|
197
|
double
|
joosttromp |
0:62fa44dd600b
|
198
|
GPS_Time::siderealDegrees(double jd, double longitude) {
|
joosttromp |
0:62fa44dd600b
|
199
|
double sidereal, gmst, lmst;
|
joosttromp |
0:62fa44dd600b
|
200
|
double T = jd - 2451545.0;
|
joosttromp |
0:62fa44dd600b
|
201
|
double T1 = T / 36525.0;
|
joosttromp |
0:62fa44dd600b
|
202
|
double T2 = T1 * T1;
|
joosttromp |
0:62fa44dd600b
|
203
|
double T3 = T2 * T1;
|
joosttromp |
0:62fa44dd600b
|
204
|
|
joosttromp |
0:62fa44dd600b
|
205
|
/* Calculate gmst angle. */
|
joosttromp |
0:62fa44dd600b
|
206
|
sidereal = 280.46061837 + (360.98564736629 * T) + (0.000387933 * T2) - (T3 / 38710000.0);
|
joosttromp |
0:62fa44dd600b
|
207
|
|
joosttromp |
0:62fa44dd600b
|
208
|
/* Convert to degrees. */
|
joosttromp |
0:62fa44dd600b
|
209
|
sidereal = fmod(sidereal, 360.0);
|
joosttromp |
0:62fa44dd600b
|
210
|
if (sidereal < 0.0) sidereal += 360.0;
|
joosttromp |
0:62fa44dd600b
|
211
|
|
joosttromp |
0:62fa44dd600b
|
212
|
gmst = sidereal;
|
joosttromp |
0:62fa44dd600b
|
213
|
lmst = gmst + longitude;
|
joosttromp |
0:62fa44dd600b
|
214
|
return lmst;
|
joosttromp |
0:62fa44dd600b
|
215
|
}
|
joosttromp |
0:62fa44dd600b
|
216
|
|
joosttromp |
0:62fa44dd600b
|
217
|
double
|
joosttromp |
0:62fa44dd600b
|
218
|
GPS_Time::siderealDegrees(GPS_Time *t, double longitude) {
|
joosttromp |
0:62fa44dd600b
|
219
|
if (t == NULL) t = new GPS_Time;
|
joosttromp |
0:62fa44dd600b
|
220
|
return siderealDegrees(julian_date(t), longitude);
|
joosttromp |
0:62fa44dd600b
|
221
|
}
|
joosttromp |
0:62fa44dd600b
|
222
|
|
joosttromp |
0:62fa44dd600b
|
223
|
double
|
joosttromp |
0:62fa44dd600b
|
224
|
GPS_Time::siderealHA(double jd, double longitude) {
|
joosttromp |
0:62fa44dd600b
|
225
|
double lmst = siderealDegrees(jd, longitude);
|
joosttromp |
0:62fa44dd600b
|
226
|
return lmst / 360.0 * 24.0;
|
joosttromp |
0:62fa44dd600b
|
227
|
}
|
joosttromp |
0:62fa44dd600b
|
228
|
|
joosttromp |
0:62fa44dd600b
|
229
|
double
|
joosttromp |
0:62fa44dd600b
|
230
|
GPS_Time::siderealHA(GPS_Time *t, double longitude) {
|
joosttromp |
0:62fa44dd600b
|
231
|
double lmst = siderealDegrees(t, longitude);
|
joosttromp |
0:62fa44dd600b
|
232
|
return lmst / 360.0 * 24.0;
|
joosttromp |
0:62fa44dd600b
|
233
|
}
|
joosttromp |
0:62fa44dd600b
|
234
|
|