Skip to content

Commit b728b31

Browse files
vchernovvchernov
committedOct 29, 2016
Added time parsing functions
1 parent 95850c5 commit b728b31

File tree

3 files changed

+107
-44
lines changed

3 files changed

+107
-44
lines changed
 

‎globpos/gpgga.cpp

+94-42
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
#include "gpgga.h"
22

3-
#include <iostream>
3+
#include <cstring>
44

55
namespace globpos {
66

@@ -11,37 +11,29 @@ bool parseGPGGA(NmeaSentence* sentence, GlobPosDegMin& pos) {
1111
int qualityVal = 0;
1212
sscanf(quality.c_str(), "%d", &qualityVal);
1313
if (qualityVal < 1) {
14-
#ifdef GLOBPOS_PRINT_ERRORS
15-
fprintf(stdout, "Low signal quality: %d, skipping sentence\n", qualityVal);
16-
#endif
1714
return false;
1815
}
1916
} else {
20-
#ifdef GLOBPOS_PRINT_ERRORS
21-
fprintf(stderr, "Unknown signal quality\n");
22-
#endif
2317
return false;
2418
}
2519

2620
// time
2721
std::string time = sentence->safeGetData(0);
2822
if (!time.empty()) {
29-
//TODO: parse timestamp
23+
if (!parseTime(time.c_str(), pos.timestamp)) {
24+
return false;
25+
}
3026
} else {
31-
#ifdef GLOBPOS_PRINT_ERRORS
32-
fprintf(stderr, "Unknown time\n");
33-
#endif
3427
return false;
3528
}
3629

3730
// latitude
3831
std::string latitude = sentence->safeGetData(1);
3932
if (!latitude.empty()) {
40-
parseLatitude(latitude, pos.latitude);
33+
if (!parseLatitude(latitude, pos.latitude)) {
34+
return false;
35+
}
4136
} else {
42-
#ifdef GLOBPOS_PRINT_ERRORS
43-
fprintf(stderr, "Unknown latitude\n");
44-
#endif
4537
return false;
4638
}
4739
std::string latitudeDir = sentence->safeGetData(2);
@@ -51,26 +43,19 @@ bool parseGPGGA(NmeaSentence* sentence, GlobPosDegMin& pos) {
5143
else if (latitudeDir == "S")
5244
pos.latitude.direction = false;
5345
else {
54-
#ifdef GLOBPOS_PRINT_ERRORS
55-
fprintf(stderr, "Unknown latitude direction\n");
56-
#endif
5746
return false;
5847
}
5948
} else {
60-
#ifdef GLOBPOS_PRINT_ERRORS
61-
fprintf(stderr, "Unknown latitude direction\n");
62-
#endif
6349
return false;
6450
}
6551

6652
// longitude
6753
std::string longitude = sentence->safeGetData(3);
6854
if (!longitude.empty()) {
69-
parseLongitude(longitude, pos.longitude);
55+
if (!parseLongitude(longitude, pos.longitude)) {
56+
return false;
57+
}
7058
} else {
71-
#ifdef GLOBPOS_PRINT_ERRORS
72-
fprintf(stderr, "Unknown longitude\n");
73-
#endif
7459
return false;
7560
}
7661
std::string longitudeDir = sentence->safeGetData(4);
@@ -80,38 +65,105 @@ bool parseGPGGA(NmeaSentence* sentence, GlobPosDegMin& pos) {
8065
else if (longitudeDir == "W")
8166
pos.longitude.direction = false;
8267
else {
83-
#ifdef GLOBPOS_PRINT_ERRORS
84-
fprintf(stderr, "Unknown longitude direction\n");
85-
#endif
8668
return false;
8769
}
8870
} else {
89-
#ifdef GLOBPOS_PRINT_ERRORS
90-
fprintf(stderr, "Unknown longitude direction\n");
91-
#endif
9271
return false;
9372
}
9473

9574
return true;
9675
}
9776

98-
bool parseCoordinate(const std::string& str, const char* format, GpsCoordDegMin& coord) {
99-
if (sscanf(str.c_str(), format, &coord.deg, &coord.minInt, &coord.minFract) == 3)
100-
return true;
101-
else {
102-
#ifdef GLOBPOS_PRINT_ERRORS
103-
fprintf(stderr, "wrong coordinate string: %s\n", str.c_str());
104-
#endif
77+
bool parseLatitude(const std::string& str, GpsCoordDegMin& coord) {
78+
return sscanf(str.c_str(), "%02d%02d.%d", &coord.deg, &coord.minInt, &coord.minFract) == 3;
79+
}
80+
81+
bool parseLongitude(const std::string& str, GpsCoordDegMin& coord) {
82+
return sscanf(str.c_str(), "%03d%02d.%d", &coord.deg, &coord.minInt, &coord.minFract) == 3;
83+
}
84+
85+
bool parseDateTime(const char* str, std::chrono::time_point<std::chrono::system_clock>& timestamp) {
86+
std::tm tm;
87+
memset(&tm, 0, sizeof(tm));
88+
89+
long millis;
90+
int filled = sscanf(str, "%04d%*c%02d%*c%02d%*c%02d%*c%02d%*c%02d%*c%06ld",
91+
&tm.tm_year, &tm.tm_mon, &tm.tm_mday,
92+
&tm.tm_hour, &tm.tm_min, &tm.tm_sec,
93+
&millis);
94+
if (filled < 6) {
10595
return false;
10696
}
97+
98+
tm.tm_mon -= 1;
99+
tm.tm_year -= 1900;
100+
if (tm.tm_year < 70) {
101+
tm.tm_year = 70;
102+
}
103+
104+
time_t sec;
105+
if (tm.tm_year == 70 && tm.tm_mon == 0 && tm.tm_mday == 1) {
106+
sec = tm.tm_hour * 60 * 60 + tm.tm_min * 60 + tm.tm_sec;
107+
} else {
108+
sec = timegm(&tm);
109+
}
110+
111+
timestamp = std::chrono::system_clock::from_time_t(sec);
112+
113+
if (filled == 7) {
114+
timestamp += std::chrono::milliseconds(millis);
115+
}
116+
return true;
107117
}
108118

109-
bool parseLatitude(const std::string& str, GpsCoordDegMin& coord) {
110-
return parseCoordinate(str, "%02d%02d.%d", coord);
119+
/// Searches for decimal point and returns number of fractional digits.
120+
int fractDigitsCnt(const std::string& numStr) {
121+
size_t p = numStr.rfind('.');
122+
if (p == std::string::npos) {
123+
return 0;
124+
}
125+
return (int)(numStr.length() - p - 1);
111126
}
112127

113-
bool parseLongitude(const std::string& str, GpsCoordDegMin& coord) {
114-
return parseCoordinate(str, "%03d%02d.%d", coord);
128+
/// Compile-time power computation.
129+
template<int N, int P> struct Power
130+
{
131+
enum
132+
{
133+
val = N * Power<N, P - 1>::val
134+
};
135+
};
136+
template<int N> struct Power<N, 0>
137+
{
138+
enum
139+
{
140+
val = 1
141+
};
142+
};
143+
144+
/// Run-time power of 10 computation.
145+
long pow10(int p) {
146+
return (p == 0) ? 1 : 10 * pow10(p - 1);
147+
}
148+
149+
bool parseTime(const char* str, std::chrono::time_point<std::chrono::system_clock>& timestamp) {
150+
int hours, min, sec;
151+
long millis;
152+
int filled = sscanf(str, "%02d%02d%02d.%ld", &hours, &min, &sec, &millis);
153+
if (filled < 3) {
154+
return false;
155+
}
156+
157+
timestamp += std::chrono::hours(hours);
158+
timestamp += std::chrono::minutes(min);
159+
timestamp += std::chrono::seconds(sec);
160+
161+
if (filled == 4) {
162+
int fractDigits = fractDigitsCnt(str);
163+
long order = Power<10, 6>::val / pow10(fractDigits);
164+
timestamp += std::chrono::milliseconds(millis * order);
165+
}
166+
return true;
115167
}
116168

117169
}

‎globpos/gpgga.h

+3
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,9 @@ bool parseLatitude(const std::string& str, GpsCoordDegMin& coord);
1515
// parses longitude from string
1616
bool parseLongitude(const std::string& str, GpsCoordDegMin& coord);
1717

18+
bool parseDateTime(const char* str, std::chrono::time_point<std::chrono::system_clock>& timestamp);
19+
bool parseTime(const char* str, std::chrono::time_point<std::chrono::system_clock>& timestamp);
20+
1821
}
1922

2023
#endif //GLOBPOS_GPGGA_H

‎tests/nmeaparser.cpp

+10-2
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
#include "../globpos/NmeaParser.h"
22

33
#include <iostream>
4-
#include <memory>
4+
#include <iomanip>
55

66
#include "../globpos/gpgga.h"
77

@@ -21,6 +21,9 @@ int main(int argc, char** argv) {
2121
return 1;
2222
}
2323

24+
std::chrono::time_point<std::chrono::system_clock> recordDate;
25+
parseDateTime("2016.10.29 00:00:00", recordDate);
26+
2427
auto parser = std::make_unique<NmeaParser>();
2528

2629
const size_t bufferLength = 1024;
@@ -30,14 +33,19 @@ int main(int argc, char** argv) {
3033
parser->parse(buffer, bytesRead);
3134

3235
for (const auto& sentence : parser->getSentences()) {
33-
std::cout << sentence->address << std::endl;
36+
std::cout << sentence->address;
3437

3538
// GPS coordinates
3639
if (sentence->address == "GPGGA") {
3740
GlobPosDegMin pos;
41+
pos.timestamp = recordDate;
3842
if (parseGPGGA(sentence.get(), pos)) {
43+
time_t t = std::chrono::system_clock::to_time_t(pos.timestamp);
44+
std::cout << " (" << std::put_time(std::gmtime(&t), "%Y.%m.%d %H:%M:%S") << ")";
3945
}
4046
}
47+
48+
std::cout << std::endl;
4149
}
4250
parser->clearSentences();
4351
}

0 commit comments

Comments
 (0)
Please sign in to comment.