Line | Branch | Decision | Exec | Source |
---|---|---|---|---|
1 | /** | |||
2 | * @date Dec 20, 2013 | |||
3 | * | |||
4 | * @author Andrey Belomutskiy, (c) 2012-2020 | |||
5 | * @author Kot_dnz | |||
6 | * | |||
7 | * This file is part of rusEfi - see http://rusefi.com | |||
8 | * | |||
9 | * rusEfi is free software; you can redistribute it and/or modify it under the terms of | |||
10 | * the GNU General Public License as published by the Free Software Foundation; either | |||
11 | * version 3 of the License, or (at your option) any later version. | |||
12 | * | |||
13 | * rusEfi is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without | |||
14 | * even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |||
15 | * GNU General Public License for more details. | |||
16 | * | |||
17 | * You should have received a copy of the GNU General Public License along with this program. | |||
18 | * If not, see <http://www.gnu.org/licenses/>. | |||
19 | * | |||
20 | * see #testGpsParser | |||
21 | */ | |||
22 | #include "pch.h" | |||
23 | #include "nmea.h" | |||
24 | #include "rtc_helper.h" | |||
25 | ||||
26 | using namespace rusefi::stringutil; | |||
27 | ||||
28 | 4 | static long hex2int(const char * a, const int len) { | ||
29 | int i; | |||
30 | 4 | long val = 0; | ||
31 | ||||
32 |
2/2✓ Branch 0 taken 8 times.
✓ Branch 1 taken 4 times.
|
2/2✓ Decision 'true' taken 8 times.
✓ Decision 'false' taken 4 times.
|
12 | for (i = 0; i < len; i++) |
33 |
1/2✓ Branch 0 taken 8 times.
✗ Branch 1 not taken.
|
1/2✓ Decision 'true' taken 8 times.
✗ Decision 'false' not taken.
|
8 | if (a[i] <= 57) |
34 | 8 | val += (a[i] - 48) * (1 << (4 * (len - 1 - i))); // it's decimal number | ||
35 | else | |||
36 | ✗ | val += (a[i] - 87) * (1 << (4 * (len - 1 - i))); // it's a-f -> work only with low case hex | ||
37 | 4 | return val; | ||
38 | } | |||
39 | ||||
40 | 2 | static float gps_deg_dec(float deg_point) { | ||
41 | 2 | float ddeg; | ||
42 | 2 | float sec = modff(deg_point, &ddeg) * 60; | ||
43 | 2 | int deg = (int) (ddeg / 100); | ||
44 | 2 | int min = (int) (deg_point - (deg * 100)); | ||
45 | ||||
46 | 2 | float absdlat = round(deg * 1000000.); | ||
47 | 2 | float absmlat = round(min * 1000000.); | ||
48 | 2 | float absslat = round(sec * 1000000.); | ||
49 | ||||
50 | 2 | return round(absdlat + (absmlat / 60) + (absslat / 3600)) / 1000000; | ||
51 | } | |||
52 | ||||
53 | // Convert lat e lon to decimals (from deg) | |||
54 | 1 | static void gps_convert_deg_to_dec(float *latitude, char ns, float *longitude, char we) { | ||
55 |
1/2✓ Branch 0 taken 1 time.
✗ Branch 1 not taken.
|
1 | float lat = (ns == 'N') ? *latitude : -1 * (*latitude); | |
56 |
1/2✓ Branch 0 taken 1 time.
✗ Branch 1 not taken.
|
1 | float lon = (we == 'E') ? *longitude : -1 * (*longitude); | |
57 | ||||
58 | 1 | *latitude = gps_deg_dec(lat); | ||
59 | 1 | *longitude = gps_deg_dec(lon); | ||
60 | 1 | } | ||
61 | ||||
62 | // in string collect all char till comma and convert to float | |||
63 | 26 | static int str_till_comma(const char * const a, char * const dStr) { | ||
64 | ||||
65 | 26 | int i = 0, sLen = strlen(a); | ||
66 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 26 times.
|
1/2✗ Decision 'true' not taken.
✓ Decision 'false' taken 26 times.
|
26 | if (sLen > GPS_MAX_STRING) |
67 | ✗ | sLen = GPS_MAX_STRING; | ||
68 | ||||
69 |
3/4✓ Branch 0 taken 164 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 138 times.
✓ Branch 3 taken 26 times.
|
0/1? Decision couldn't be analyzed.
|
164 | while (i < sLen && a[i] != 44) { // while not comma or end |
70 | 138 | dStr[i] = a[i]; | ||
71 | 138 | i++; | ||
72 | } | |||
73 | 26 | dStr[i] = '\0'; | ||
74 | 26 | return i; | ||
75 | } | |||
76 | ||||
77 | /* | |||
78 | GxGGA - name code | |||
79 | Parameter Value Unit Description | |||
80 | UTC hhmmss.sss Universal time coordinated | |||
81 | Lat ddmm.mmmm Latitude | |||
82 | Northing Indicator N=North, S=South | |||
83 | Lon dddmm.mmmm Longitude | |||
84 | Easting Indicator E=East, W=West | |||
85 | Status 0 0=Invalid, 1=2D/3D, 2=DGPS, 6=Dead Reckoning | |||
86 | SVs Used 00 Number of SVs used for Navigation | |||
87 | HDOP 99.99 Horizontal Dilution of Precision | |||
88 | Alt (MSL) m Altitude (above means sea level) | |||
89 | Unit M=Meters | |||
90 | Geoid Sep. m Geoid Separation = Alt(HAE) - Alt(MSL) | |||
91 | Unit M=Meters | |||
92 | Age of DGPS Corr s Age of Differential Corrections | |||
93 | DGPS Ref Station ID of DGPS Reference Station | |||
94 | */ | |||
95 | 1 | static void nmea_parse_gpgga(char const * const nmea, loc_t *loc) { | ||
96 | 1 | char const * p = (char *)nmea; | ||
97 | 1 | char dStr[GPS_MAX_STRING]; | ||
98 | ||||
99 | 1 | p = strchr(p, ',') + 1; //skip time - we read date&time if Valid in GxRMC | ||
100 | ||||
101 | 1 | p = strchr(p, ',') + 1; // in p string started with searching address | ||
102 | 1 | str_till_comma(p, dStr); // str to float till comma saved modified string | ||
103 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1 time.
|
1/2✗ Decision 'true' not taken.
✓ Decision 'false' taken 1 time.
|
1 | if (strlen(p) == 0) { |
104 | ✗ | return; // if no data in field - empty data - we return | ||
105 | } | |||
106 | ||||
107 |
1/1✓ Branch 1 taken 1 time.
|
1 | loc->latitude = atoff(dStr); // fulfil data | |
108 | ||||
109 | 1 | p = strchr(p, ',') + 1; // see above | ||
110 |
1/4✓ Branch 0 taken 1 time.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
1 | switch (p[0]) { | |
111 |
1/1✓ Decision 'true' taken 1 time.
|
1 | case 'N': | |
112 | 1 | loc->lat = 'N'; | ||
113 | 1 | break; | ||
114 | ✗ | case 'S': | ||
115 | ✗ | loc->lat = 'S'; | ||
116 | ✗ | break; | ||
117 | ✗ | case ',': | ||
118 | ✗ | loc->lat = '\0'; | ||
119 | ✗ | break; | ||
120 | } | |||
121 | ||||
122 | 1 | p = strchr(p, ',') + 1; | ||
123 | 1 | str_till_comma(p, dStr); // str to float till comma saved modified string | ||
124 |
1/1✓ Branch 1 taken 1 time.
|
1 | loc->longitude = atoff(dStr); | |
125 | ||||
126 | 1 | p = strchr(p, ',') + 1; | ||
127 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 1 time.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
1 | switch (p[0]) { | |
128 | ✗ | case 'W': | ||
129 | ✗ | loc->lon = 'W'; | ||
130 | ✗ | break; | ||
131 |
1/1✓ Decision 'true' taken 1 time.
|
1 | case 'E': | |
132 | 1 | loc->lon = 'E'; | ||
133 | 1 | break; | ||
134 | ✗ | case ',': | ||
135 | ✗ | loc->lon = '\0'; | ||
136 | ✗ | break; | ||
137 | } | |||
138 | ||||
139 | 1 | p = strchr(p, ',') + 1; | ||
140 | 1 | str_till_comma(p, dStr); // str to float till comma saved modified string | ||
141 | 1 | loc->quality = atoi(dStr); | ||
142 | ||||
143 | 1 | p = strchr(p, ',') + 1; | ||
144 | 1 | str_till_comma(p, dStr); // str to float till comma saved modified string | ||
145 | 1 | loc->satellites = atoi(dStr); | ||
146 | ||||
147 | 1 | p = strchr(p, ',') + 1; | ||
148 | ||||
149 | 1 | p = strchr(p, ',') + 1; | ||
150 | 1 | str_till_comma(p, dStr); // str to float till comma saved modified string | ||
151 |
1/1✓ Branch 1 taken 1 time.
|
1 | loc->altitude = atoff(dStr); | |
152 | } | |||
153 | ||||
154 | /* | |||
155 | GxRMC - nmea code | |||
156 | Parameter Value Unit Description | |||
157 | UTC hhmmss.sss Universal time coordinated | |||
158 | Status V A=Valid, V=Invalid | |||
159 | Lat ddmm.mmmm Latitude | |||
160 | Northing Indicator N=North, S=South | |||
161 | Lon dddmm.mmmm Longitude | |||
162 | Easting Indicator E=East, W=West | |||
163 | SOG nots Speed Over Ground | |||
164 | COG (true) ° Course Over Ground (true) | |||
165 | Date ddmmyy Universal time coordinated | |||
166 | Magnetic Variation ° Magnetic Variation | |||
167 | Magnetic Variation E=East,W=West | |||
168 | Mode Indicator N A=Autonomous, D=Differential, E=Dead Reckoning, N=None | |||
169 | Navigational Status S=Safe C=Caution U=Unsafe V=Not valid | |||
170 | */ | |||
171 | ||||
172 | 3 | static void nmea_parse_gprmc(char const * const nmea, loc_t *loc) { | ||
173 | 3 | char const * p = (char *)nmea; | ||
174 | 3 | char dStr[GPS_MAX_STRING]; | ||
175 | 3 | efidatetime_t dt; | ||
176 | ||||
177 | 3 | p = strchr(p, ',') + 1; // read time | ||
178 | 3 | str_till_comma(p, dStr); | ||
179 |
1/2✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
|
1/2✓ Decision 'true' taken 3 times.
✗ Decision 'false' not taken.
|
3 | if (strlen(dStr) > 5) { |
180 | 3 | dt.hour = str2int(dStr, 2); | ||
181 | 3 | dt.minute = str2int(dStr + 2, 2); | ||
182 | 3 | dt.second = str2int(dStr + 4, 2); | ||
183 | } | |||
184 | ||||
185 | 3 | p = strchr(p, ',') + 1; // read field Valid status | ||
186 | 3 | str_till_comma(p, dStr); | ||
187 | ||||
188 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
|
1/2✗ Decision 'true' not taken.
✓ Decision 'false' taken 3 times.
|
3 | if (dStr[0] == 'V') { // if field is invalid |
189 | ✗ | loc->quality = 0; | ||
190 | ✗ | return; | ||
191 | } | |||
192 | ||||
193 | 3 | loc->quality = 4; // this is declaration that last receive field VALID | ||
194 | ||||
195 | 3 | p = strchr(p, ',') + 1; // latitude | ||
196 | 3 | str_till_comma(p, dStr); // str to float till comma saved modified string | ||
197 |
1/1✓ Branch 1 taken 3 times.
|
3 | loc->latitude = atoff(dStr); | |
198 | ||||
199 | 3 | p = strchr(p, ',') + 1; | ||
200 |
1/4✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
3 | switch (p[0]) { | |
201 |
1/1✓ Decision 'true' taken 3 times.
|
3 | case 'N': | |
202 | 3 | loc->lat = 'N'; | ||
203 | 3 | break; | ||
204 | ✗ | case 'S': | ||
205 | ✗ | loc->lat = 'S'; | ||
206 | ✗ | break; | ||
207 | ✗ | case ',': | ||
208 | ✗ | loc->lat = '\0'; | ||
209 | ✗ | break; | ||
210 | } | |||
211 | ||||
212 | 3 | p = strchr(p, ',') + 1; // longitude | ||
213 | 3 | str_till_comma(p, dStr); // str to float till comma saved modified string | ||
214 |
1/1✓ Branch 1 taken 3 times.
|
3 | loc->longitude = atoff(dStr); | |
215 | ||||
216 | 3 | p = strchr(p, ',') + 1; | ||
217 |
2/4✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 time.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
3 | switch (p[0]) { | |
218 |
1/1✓ Decision 'true' taken 2 times.
|
2 | case 'W': | |
219 | 2 | loc->lon = 'W'; | ||
220 | 2 | break; | ||
221 |
1/1✓ Decision 'true' taken 1 time.
|
1 | case 'E': | |
222 | 1 | loc->lon = 'E'; | ||
223 | 1 | break; | ||
224 | ✗ | case ',': | ||
225 | ✗ | loc->lon = '\0'; | ||
226 | ✗ | break; | ||
227 | } | |||
228 | ||||
229 | 3 | p = strchr(p, ',') + 1; | ||
230 | 3 | str_till_comma(p, dStr); // str to float till comma saved modified string | ||
231 |
1/1✓ Branch 1 taken 3 times.
|
3 | loc->speed = atoff(dStr); | |
232 | ||||
233 | 3 | p = strchr(p, ',') + 1; | ||
234 | 3 | str_till_comma(p, dStr); // str to float till comma saved modified string | ||
235 |
1/1✓ Branch 1 taken 3 times.
|
3 | loc->course = atoff(dStr); | |
236 | ||||
237 | 3 | p = strchr(p, ',') + 1; // read date | ||
238 | 3 | str_till_comma(p, dStr); | ||
239 |
1/2✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
|
1/2✓ Decision 'true' taken 3 times.
✗ Decision 'false' not taken.
|
3 | if (strlen(dStr) > 5) { |
240 | 3 | dt.day = str2int(dStr, 2); | ||
241 | 3 | dt.month = str2int(dStr + 2, 2); | ||
242 | 3 | dt.year = 100 + // we receive -200, but standard wait -1900 = add correction | ||
243 | 3 | str2int(dStr + 4, 2); | ||
244 | } | |||
245 | ||||
246 |
1/2✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
|
1/2✓ Decision 'true' taken 3 times.
✗ Decision 'false' not taken.
|
3 | if (dt.year > 0 ) { // check if date field is valid |
247 | 3 | memcpy(&loc->time, &dt, sizeof(dt)); | ||
248 | } | |||
249 | } | |||
250 | ||||
251 | /** | |||
252 | * Get the message type (GPGGA, GPRMC, etc..) | |||
253 | * | |||
254 | * This function filters out also wrong packages (invalid checksum) | |||
255 | * | |||
256 | * @param message The NMEA message | |||
257 | * @return The type of message if it is valid | |||
258 | */ | |||
259 | 5 | nmea_message_type nmea_get_message_type(const char *message) { | ||
260 | 5 | int checksum = nmea_valid_checksum(message); | ||
261 |
2/2✓ Branch 0 taken 1 time.
✓ Branch 1 taken 4 times.
|
2/2✓ Decision 'true' taken 1 time.
✓ Decision 'false' taken 4 times.
|
5 | if (checksum != _EMPTY) { |
262 | 1 | return static_cast<nmea_message_type>(checksum); | ||
263 | } | |||
264 | ||||
265 |
2/2✓ Branch 0 taken 1 time.
✓ Branch 1 taken 3 times.
|
2/2✓ Decision 'true' taken 1 time.
✓ Decision 'false' taken 3 times.
|
4 | if (strstr(message, NMEA_GPGGA_STR) != NULL) { |
266 | 1 | return NMEA_GPGGA; | ||
267 | } | |||
268 | ||||
269 |
1/2✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
|
1/2✓ Decision 'true' taken 3 times.
✗ Decision 'false' not taken.
|
3 | if (strstr(message, NMEA_GPRMC_STR) != NULL) { |
270 | 3 | return NMEA_GPRMC; | ||
271 | } | |||
272 | ||||
273 | ✗ | return NMEA_UNKNOWN; | ||
274 | } | |||
275 | ||||
276 | 5 | int nmea_valid_checksum(char const * message) { | ||
277 | char p; | |||
278 | 5 | int sum = 0; | ||
279 | 5 | const char* starPtr = strrchr(message, '*'); | ||
280 |
2/2✓ Branch 0 taken 1 time.
✓ Branch 1 taken 4 times.
|
2/2✓ Decision 'true' taken 1 time.
✓ Decision 'false' taken 4 times.
|
5 | if (!starPtr) { |
281 | 1 | return NMEA_CHECKSUM_ERR; | ||
282 | } | |||
283 | 4 | const char* int_message = starPtr + 1; | ||
284 | 4 | long checksum = hex2int(int_message, 2); | ||
285 | ||||
286 | 4 | ++message; | ||
287 |
2/2✓ Branch 0 taken 253 times.
✓ Branch 1 taken 4 times.
|
2/2✓ Decision 'true' taken 253 times.
✓ Decision 'false' taken 4 times.
|
257 | while ((p = *message++) != '*') { |
288 | 253 | sum ^= p; | ||
289 | } | |||
290 | ||||
291 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
|
1/2✗ Decision 'true' not taken.
✓ Decision 'false' taken 4 times.
|
4 | if (sum != checksum) { |
292 | ✗ | return NMEA_CHECKSUM_ERR; | ||
293 | } | |||
294 | 4 | return _EMPTY; | ||
295 | } | |||
296 | ||||
297 | // Compute the GPS location using decimal scale | |||
298 | 5 | void gps_location(loc_t *coord, char const * const buffer) { | ||
299 | 5 | coord->type = nmea_get_message_type(buffer); | ||
300 | ||||
301 |
3/4✓ Branch 0 taken 1 time.
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 time.
|
5 | switch (coord->type) { | |
302 |
1/1✓ Decision 'true' taken 1 time.
|
1 | case NMEA_GPGGA: | |
303 | 1 | nmea_parse_gpgga(buffer, coord); | ||
304 | 1 | gps_convert_deg_to_dec(&(coord->latitude), coord->lat, &(coord->longitude), coord->lon); | ||
305 | 1 | break; | ||
306 |
1/1✓ Decision 'true' taken 3 times.
|
3 | case NMEA_GPRMC: | |
307 | 3 | nmea_parse_gprmc(buffer, coord); | ||
308 | 3 | break; | ||
309 | ✗ | case NMEA_UNKNOWN: | ||
310 | // unknown message type | |||
311 | ✗ | break; | ||
312 | } | |||
313 | 5 | } | ||
314 |