1 /*
2 * Copyright (c) 2023 Trackunit Corporation
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/kernel.h>
8
9 #include <string.h>
10 #include <stdarg.h>
11 #include <stdarg.h>
12
13 #include "gnss_nmea0183.h"
14 #include "gnss_parse.h"
15
16 #define GNSS_NMEA0183_PICO_DEGREES_IN_DEGREE (1000000000000ULL)
17 #define GNSS_NMEA0183_PICO_DEGREES_IN_MINUTE (GNSS_NMEA0183_PICO_DEGREES_IN_DEGREE / 60ULL)
18 #define GNSS_NMEA0183_PICO_DEGREES_IN_NANO_DEGREE (1000ULL)
19 #define GNSS_NMEA0183_NANO_KNOTS_IN_MMS (1943861LL)
20
21 #define GNSS_NMEA0183_MESSAGE_SIZE_MIN (6)
22 #define GNSS_NMEA0183_MESSAGE_CHECKSUM_SIZE (3)
23
24 #define GNSS_NMEA0183_GSV_HDR_ARG_CNT (4)
25 #define GNSS_NMEA0183_GSV_SV_ARG_CNT (4)
26
27 #define GNSS_NMEA0183_GSV_PRN_GPS_RANGE (32)
28 #define GNSS_NMEA0183_GSV_PRN_SBAS_OFFSET (87)
29 #define GNSS_NMEA0183_GSV_PRN_GLONASS_OFFSET (64)
30 #define GNSS_NMEA0183_GSV_PRN_BEIDOU_OFFSET (100)
31
32 struct gsv_header_args {
33 const char *message_id;
34 const char *number_of_messages;
35 const char *message_number;
36 const char *numver_of_svs;
37 };
38
39 struct gsv_sv_args {
40 const char *prn;
41 const char *elevation;
42 const char *azimuth;
43 const char *snr;
44 };
45
gnss_system_from_gsv_header_args(const struct gsv_header_args * args,enum gnss_system * sv_system)46 static int gnss_system_from_gsv_header_args(const struct gsv_header_args *args,
47 enum gnss_system *sv_system)
48 {
49 switch (args->message_id[2]) {
50 case 'A':
51 *sv_system = GNSS_SYSTEM_GALILEO;
52 break;
53 case 'B':
54 *sv_system = GNSS_SYSTEM_BEIDOU;
55 break;
56 case 'P':
57 *sv_system = GNSS_SYSTEM_GPS;
58 break;
59 case 'L':
60 *sv_system = GNSS_SYSTEM_GLONASS;
61 break;
62 case 'Q':
63 *sv_system = GNSS_SYSTEM_QZSS;
64 break;
65 default:
66 return -EINVAL;
67 }
68
69 return 0;
70 }
71
align_satellite_with_gnss_system(enum gnss_system sv_system,struct gnss_satellite * satellite)72 static void align_satellite_with_gnss_system(enum gnss_system sv_system,
73 struct gnss_satellite *satellite)
74 {
75 switch (sv_system) {
76 case GNSS_SYSTEM_GPS:
77 if (satellite->prn > GNSS_NMEA0183_GSV_PRN_GPS_RANGE) {
78 satellite->system = GNSS_SYSTEM_SBAS;
79 satellite->prn += GNSS_NMEA0183_GSV_PRN_SBAS_OFFSET;
80 break;
81 }
82
83 satellite->system = GNSS_SYSTEM_GPS;
84 break;
85
86 case GNSS_SYSTEM_GLONASS:
87 satellite->system = GNSS_SYSTEM_GLONASS;
88 satellite->prn -= GNSS_NMEA0183_GSV_PRN_GLONASS_OFFSET;
89 break;
90
91 case GNSS_SYSTEM_GALILEO:
92 satellite->system = GNSS_SYSTEM_GALILEO;
93 break;
94
95 case GNSS_SYSTEM_BEIDOU:
96 satellite->system = GNSS_SYSTEM_BEIDOU;
97 satellite->prn -= GNSS_NMEA0183_GSV_PRN_BEIDOU_OFFSET;
98 break;
99
100 case GNSS_SYSTEM_QZSS:
101 satellite->system = GNSS_SYSTEM_QZSS;
102 break;
103
104 case GNSS_SYSTEM_IRNSS:
105 case GNSS_SYSTEM_IMES:
106 case GNSS_SYSTEM_SBAS:
107 break;
108 }
109 }
110
gnss_nmea0183_checksum(const char * str)111 uint8_t gnss_nmea0183_checksum(const char *str)
112 {
113 uint8_t checksum = 0;
114 size_t end;
115
116 __ASSERT(str != NULL, "str argument must be provided");
117
118 end = strlen(str);
119 for (size_t i = 0; i < end; i++) {
120 checksum = checksum ^ str[i];
121 }
122
123 return checksum;
124 }
125
gnss_nmea0183_snprintk(char * str,size_t size,const char * fmt,...)126 int gnss_nmea0183_snprintk(char *str, size_t size, const char *fmt, ...)
127 {
128 va_list ap;
129 uint8_t checksum;
130 int pos;
131 int len;
132
133 __ASSERT(str != NULL, "str argument must be provided");
134 __ASSERT(fmt != NULL, "fmt argument must be provided");
135
136 if (size < GNSS_NMEA0183_MESSAGE_SIZE_MIN) {
137 return -ENOMEM;
138 }
139
140 str[0] = '$';
141
142 va_start(ap, fmt);
143 pos = vsnprintk(&str[1], size - 1, fmt, ap) + 1;
144 va_end(ap);
145
146 if (pos < 0) {
147 return -EINVAL;
148 }
149
150 len = pos + GNSS_NMEA0183_MESSAGE_CHECKSUM_SIZE;
151
152 if ((size - 1) < len) {
153 return -ENOMEM;
154 }
155
156 checksum = gnss_nmea0183_checksum(&str[1]);
157 pos = snprintk(&str[pos], size - pos, "*%02X", checksum);
158 if (pos != 3) {
159 return -EINVAL;
160 }
161
162 str[len] = '\0';
163 return len;
164 }
165
gnss_nmea0183_ddmm_mmmm_to_ndeg(const char * ddmm_mmmm,int64_t * ndeg)166 int gnss_nmea0183_ddmm_mmmm_to_ndeg(const char *ddmm_mmmm, int64_t *ndeg)
167 {
168 uint64_t pico_degrees = 0;
169 int8_t decimal = -1;
170 int8_t pos = 0;
171 uint64_t increment;
172
173 __ASSERT(ddmm_mmmm != NULL, "ddmm_mmmm argument must be provided");
174 __ASSERT(ndeg != NULL, "ndeg argument must be provided");
175
176 /* Find decimal */
177 while (ddmm_mmmm[pos] != '\0') {
178 /* Verify if char is decimal */
179 if (ddmm_mmmm[pos] == '.') {
180 decimal = pos;
181 break;
182 }
183
184 /* Advance position */
185 pos++;
186 }
187
188 /* Verify decimal was found and placed correctly */
189 if (decimal < 1) {
190 return -EINVAL;
191 }
192
193 /* Validate potential degree fraction is within bounds */
194 if (decimal > 1 && ddmm_mmmm[decimal - 2] > '5') {
195 return -EINVAL;
196 }
197
198 /* Convert minute fraction to pico degrees and add it to pico_degrees */
199 pos = decimal + 1;
200 increment = (GNSS_NMEA0183_PICO_DEGREES_IN_MINUTE / 10);
201 while (ddmm_mmmm[pos] != '\0') {
202 /* Verify char is decimal */
203 if (ddmm_mmmm[pos] < '0' || ddmm_mmmm[pos] > '9') {
204 return -EINVAL;
205 }
206
207 /* Add increment to pico_degrees */
208 pico_degrees += (ddmm_mmmm[pos] - '0') * increment;
209
210 /* Update unit */
211 increment /= 10;
212
213 /* Increment position */
214 pos++;
215 }
216
217 /* Convert minutes and degrees to pico_degrees */
218 pos = decimal - 1;
219 increment = GNSS_NMEA0183_PICO_DEGREES_IN_MINUTE;
220 while (pos >= 0) {
221 /* Check if digit switched from minutes to degrees */
222 if ((decimal - pos) == 3) {
223 /* Reset increment to degrees */
224 increment = GNSS_NMEA0183_PICO_DEGREES_IN_DEGREE;
225 }
226
227 /* Verify char is decimal */
228 if (ddmm_mmmm[pos] < '0' || ddmm_mmmm[pos] > '9') {
229 return -EINVAL;
230 }
231
232 /* Add increment to pico_degrees */
233 pico_degrees += (ddmm_mmmm[pos] - '0') * increment;
234
235 /* Update unit */
236 increment *= 10;
237
238 /* Decrement position */
239 pos--;
240 }
241
242 /* Convert to nano degrees */
243 *ndeg = (int64_t)(pico_degrees / GNSS_NMEA0183_PICO_DEGREES_IN_NANO_DEGREE);
244 return 0;
245 }
246
gnss_nmea0183_validate_message(char ** argv,uint16_t argc)247 bool gnss_nmea0183_validate_message(char **argv, uint16_t argc)
248 {
249 int32_t tmp = 0;
250 uint8_t checksum = 0;
251 size_t len;
252
253 __ASSERT(argv != NULL, "argv argument must be provided");
254
255 /* Message must contain message id and checksum */
256 if (argc < 2) {
257 return false;
258 }
259
260 /* First argument should start with '$' which is not covered by checksum */
261 if ((argc < 1) || (argv[0][0] != '$')) {
262 return false;
263 }
264
265 len = strlen(argv[0]);
266 for (uint16_t u = 1; u < len; u++) {
267 checksum ^= argv[0][u];
268 }
269 checksum ^= ',';
270
271 /* Cover all except last argument which contains the checksum*/
272 for (uint16_t i = 1; i < (argc - 1); i++) {
273 len = strlen(argv[i]);
274 for (uint16_t u = 0; u < len; u++) {
275 checksum ^= argv[i][u];
276 }
277 checksum ^= ',';
278 }
279
280 if ((gnss_parse_atoi(argv[argc - 1], 16, &tmp) < 0) ||
281 (tmp > UINT8_MAX) ||
282 (tmp < 0)) {
283 return false;
284 }
285
286 return checksum == (uint8_t)tmp;
287 }
288
gnss_nmea0183_knots_to_mms(const char * str,int64_t * mms)289 int gnss_nmea0183_knots_to_mms(const char *str, int64_t *mms)
290 {
291 int ret;
292
293 __ASSERT(str != NULL, "str argument must be provided");
294 __ASSERT(mms != NULL, "mms argument must be provided");
295
296 ret = gnss_parse_dec_to_nano(str, mms);
297 if (ret < 0) {
298 return ret;
299 }
300
301 *mms = (*mms) / GNSS_NMEA0183_NANO_KNOTS_IN_MMS;
302 return 0;
303 }
304
gnss_nmea0183_parse_hhmmss(const char * hhmmss,struct gnss_time * utc)305 int gnss_nmea0183_parse_hhmmss(const char *hhmmss, struct gnss_time *utc)
306 {
307 int64_t i64;
308 int32_t i32;
309 char part[3] = {0};
310
311 __ASSERT(hhmmss != NULL, "hhmmss argument must be provided");
312 __ASSERT(utc != NULL, "utc argument must be provided");
313
314 if (strlen(hhmmss) < 6) {
315 return -EINVAL;
316 }
317
318 memcpy(part, hhmmss, 2);
319 if ((gnss_parse_atoi(part, 10, &i32) < 0) ||
320 (i32 < 0) ||
321 (i32 > 23)) {
322 return -EINVAL;
323 }
324
325 utc->hour = (uint8_t)i32;
326
327 memcpy(part, &hhmmss[2], 2);
328 if ((gnss_parse_atoi(part, 10, &i32) < 0) ||
329 (i32 < 0) ||
330 (i32 > 59)) {
331 return -EINVAL;
332 }
333
334 utc->minute = (uint8_t)i32;
335
336 if ((gnss_parse_dec_to_milli(&hhmmss[4], &i64) < 0) ||
337 (i64 < 0) ||
338 (i64 > 59999)) {
339 return -EINVAL;
340 }
341
342 utc->millisecond = (uint16_t)i64;
343 return 0;
344 }
345
gnss_nmea0183_parse_ddmmyy(const char * ddmmyy,struct gnss_time * utc)346 int gnss_nmea0183_parse_ddmmyy(const char *ddmmyy, struct gnss_time *utc)
347 {
348 int32_t i32;
349 char part[3] = {0};
350
351 __ASSERT(ddmmyy != NULL, "ddmmyy argument must be provided");
352 __ASSERT(utc != NULL, "utc argument must be provided");
353
354 if (strlen(ddmmyy) != 6) {
355 return -EINVAL;
356 }
357
358 memcpy(part, ddmmyy, 2);
359 if ((gnss_parse_atoi(part, 10, &i32) < 0) ||
360 (i32 < 1) ||
361 (i32 > 31)) {
362 return -EINVAL;
363 }
364
365 utc->month_day = (uint8_t)i32;
366
367 memcpy(part, &ddmmyy[2], 2);
368 if ((gnss_parse_atoi(part, 10, &i32) < 0) ||
369 (i32 < 1) ||
370 (i32 > 12)) {
371 return -EINVAL;
372 }
373
374 utc->month = (uint8_t)i32;
375
376 memcpy(part, &ddmmyy[4], 2);
377 if ((gnss_parse_atoi(part, 10, &i32) < 0) ||
378 (i32 < 0) ||
379 (i32 > 99)) {
380 return -EINVAL;
381 }
382
383 utc->century_year = (uint8_t)i32;
384 return 0;
385 }
386
gnss_nmea0183_parse_rmc(const char ** argv,uint16_t argc,struct gnss_data * data)387 int gnss_nmea0183_parse_rmc(const char **argv, uint16_t argc, struct gnss_data *data)
388 {
389 int64_t tmp;
390
391 __ASSERT(argv != NULL, "argv argument must be provided");
392 __ASSERT(data != NULL, "data argument must be provided");
393
394 if (argc < 10) {
395 return -EINVAL;
396 }
397
398 /* Validate GNSS has fix */
399 if (argv[2][0] == 'V') {
400 return 0;
401 }
402
403 if (argv[2][0] != 'A') {
404 return -EINVAL;
405 }
406
407 /* Parse UTC time */
408 if ((gnss_nmea0183_parse_hhmmss(argv[1], &data->utc) < 0)) {
409 return -EINVAL;
410 }
411
412 /* Validate cardinal directions */
413 if (((argv[4][0] != 'N') && (argv[4][0] != 'S')) ||
414 ((argv[6][0] != 'E') && (argv[6][0] != 'W'))) {
415 return -EINVAL;
416 }
417
418 /* Parse coordinates */
419 if ((gnss_nmea0183_ddmm_mmmm_to_ndeg(argv[3], &data->nav_data.latitude) < 0) ||
420 (gnss_nmea0183_ddmm_mmmm_to_ndeg(argv[5], &data->nav_data.longitude) < 0)) {
421 return -EINVAL;
422 }
423
424 /* Align sign of coordinates with cardinal directions */
425 data->nav_data.latitude = argv[4][0] == 'N'
426 ? data->nav_data.latitude
427 : -data->nav_data.latitude;
428
429 data->nav_data.longitude = argv[6][0] == 'E'
430 ? data->nav_data.longitude
431 : -data->nav_data.longitude;
432
433 /* Parse speed */
434 if ((gnss_nmea0183_knots_to_mms(argv[7], &tmp) < 0) ||
435 (tmp > UINT32_MAX)) {
436 return -EINVAL;
437 }
438
439 data->nav_data.speed = (uint32_t)tmp;
440
441 /* Parse bearing */
442 if ((gnss_parse_dec_to_milli(argv[8], &tmp) < 0) ||
443 (tmp > 359999) ||
444 (tmp < 0)) {
445 return -EINVAL;
446 }
447
448 data->nav_data.bearing = (uint32_t)tmp;
449
450 /* Parse UTC date */
451 if ((gnss_nmea0183_parse_ddmmyy(argv[9], &data->utc) < 0)) {
452 return -EINVAL;
453 }
454
455 return 0;
456 }
457
parse_gga_fix_quality(const char * str,enum gnss_fix_quality * fix_quality)458 static int parse_gga_fix_quality(const char *str, enum gnss_fix_quality *fix_quality)
459 {
460 __ASSERT(str != NULL, "str argument must be provided");
461 __ASSERT(fix_quality != NULL, "fix_quality argument must be provided");
462
463 if ((str[1] != ((char)'\0')) || (str[0] < ((char)'0')) || (((char)'6') < str[0])) {
464 return -EINVAL;
465 }
466
467 (*fix_quality) = (enum gnss_fix_quality)(str[0] - ((char)'0'));
468 return 0;
469 }
470
fix_status_from_fix_quality(enum gnss_fix_quality fix_quality)471 static enum gnss_fix_status fix_status_from_fix_quality(enum gnss_fix_quality fix_quality)
472 {
473 enum gnss_fix_status fix_status = GNSS_FIX_STATUS_NO_FIX;
474
475 switch (fix_quality) {
476 case GNSS_FIX_QUALITY_GNSS_SPS:
477 case GNSS_FIX_QUALITY_GNSS_PPS:
478 fix_status = GNSS_FIX_STATUS_GNSS_FIX;
479 break;
480
481 case GNSS_FIX_QUALITY_DGNSS:
482 case GNSS_FIX_QUALITY_RTK:
483 case GNSS_FIX_QUALITY_FLOAT_RTK:
484 fix_status = GNSS_FIX_STATUS_DGNSS_FIX;
485 break;
486
487 case GNSS_FIX_QUALITY_ESTIMATED:
488 fix_status = GNSS_FIX_STATUS_ESTIMATED_FIX;
489 break;
490
491 default:
492 break;
493 }
494
495 return fix_status;
496 }
497
gnss_nmea0183_parse_gga(const char ** argv,uint16_t argc,struct gnss_data * data)498 int gnss_nmea0183_parse_gga(const char **argv, uint16_t argc, struct gnss_data *data)
499 {
500 int32_t tmp32;
501 int64_t tmp64;
502
503 __ASSERT(argv != NULL, "argv argument must be provided");
504 __ASSERT(data != NULL, "data argument must be provided");
505
506 if (argc < 12) {
507 return -EINVAL;
508 }
509
510 /* Parse fix quality and status */
511 if (parse_gga_fix_quality(argv[6], &data->info.fix_quality) < 0) {
512 return -EINVAL;
513 }
514
515 data->info.fix_status = fix_status_from_fix_quality(data->info.fix_quality);
516
517 /* Validate GNSS has fix */
518 if (data->info.fix_status == GNSS_FIX_STATUS_NO_FIX) {
519 return 0;
520 }
521
522 /* Parse number of satellites */
523 if ((gnss_parse_atoi(argv[7], 10, &tmp32) < 0) ||
524 (tmp32 > UINT16_MAX) ||
525 (tmp32 < 0)) {
526 return -EINVAL;
527 }
528
529 data->info.satellites_cnt = (uint16_t)tmp32;
530
531 /* Parse HDOP */
532 if ((gnss_parse_dec_to_milli(argv[8], &tmp64) < 0) ||
533 (tmp64 > UINT32_MAX) ||
534 (tmp64 < 0)) {
535 return -EINVAL;
536 }
537
538 data->info.hdop = (uint16_t)tmp64;
539
540 /* Parse altitude */
541 if ((gnss_parse_dec_to_milli(argv[9], &tmp64) < 0) ||
542 (tmp64 > INT32_MAX) ||
543 (tmp64 < INT32_MIN)) {
544 return -EINVAL;
545 }
546
547 data->nav_data.altitude = (int32_t)tmp64;
548 return 0;
549 }
550
parse_gsv_svs(struct gnss_satellite * satellites,const struct gsv_sv_args * svs,uint16_t svs_size)551 static int parse_gsv_svs(struct gnss_satellite *satellites, const struct gsv_sv_args *svs,
552 uint16_t svs_size)
553 {
554 int32_t i32;
555
556 for (uint16_t i = 0; i < svs_size; i++) {
557 /* Parse PRN */
558 if ((gnss_parse_atoi(svs[i].prn, 10, &i32) < 0) ||
559 (i32 < 0) || (i32 > UINT16_MAX)) {
560 return -EINVAL;
561 }
562
563 satellites[i].prn = (uint16_t)i32;
564
565 /* Parse elevation */
566 if ((gnss_parse_atoi(svs[i].elevation, 10, &i32) < 0) ||
567 (i32 < 0) || (i32 > 90)) {
568 return -EINVAL;
569 }
570
571 satellites[i].elevation = (uint8_t)i32;
572
573 /* Parse azimuth */
574 if ((gnss_parse_atoi(svs[i].azimuth, 10, &i32) < 0) ||
575 (i32 < 0) || (i32 > 359)) {
576 return -EINVAL;
577 }
578
579 satellites[i].azimuth = (uint16_t)i32;
580
581 /* Parse SNR */
582 if (strlen(svs[i].snr) == 0) {
583 satellites[i].snr = 0;
584 satellites[i].is_tracked = false;
585 continue;
586 }
587
588 if ((gnss_parse_atoi(svs[i].snr, 10, &i32) < 0) ||
589 (i32 < 0) || (i32 > 99)) {
590 return -EINVAL;
591 }
592
593 satellites[i].snr = (uint16_t)i32;
594 satellites[i].is_tracked = true;
595 }
596
597 return 0;
598 }
599
gnss_nmea0183_parse_gsv_header(const char ** argv,uint16_t argc,struct gnss_nmea0183_gsv_header * header)600 int gnss_nmea0183_parse_gsv_header(const char **argv, uint16_t argc,
601 struct gnss_nmea0183_gsv_header *header)
602 {
603 const struct gsv_header_args *args = (const struct gsv_header_args *)argv;
604 int i32;
605
606 __ASSERT(argv != NULL, "argv argument must be provided");
607 __ASSERT(header != NULL, "header argument must be provided");
608
609 if (argc < 4) {
610 return -EINVAL;
611 }
612
613 /* Parse GNSS sv_system */
614 if (gnss_system_from_gsv_header_args(args, &header->system) < 0) {
615 return -EINVAL;
616 }
617
618 /* Parse number of messages */
619 if ((gnss_parse_atoi(args->number_of_messages, 10, &i32) < 0) ||
620 (i32 < 0) || (i32 > UINT16_MAX)) {
621 return -EINVAL;
622 }
623
624 header->number_of_messages = (uint16_t)i32;
625
626 /* Parse message number */
627 if ((gnss_parse_atoi(args->message_number, 10, &i32) < 0) ||
628 (i32 < 0) || (i32 > UINT16_MAX)) {
629 return -EINVAL;
630 }
631
632 header->message_number = (uint16_t)i32;
633
634 /* Parse message number */
635 if ((gnss_parse_atoi(args->numver_of_svs, 10, &i32) < 0) ||
636 (i32 < 0) || (i32 > UINT16_MAX)) {
637 return -EINVAL;
638 }
639
640 header->number_of_svs = (uint16_t)i32;
641 return 0;
642 }
643
gnss_nmea0183_parse_gsv_svs(const char ** argv,uint16_t argc,struct gnss_satellite * satellites,uint16_t size)644 int gnss_nmea0183_parse_gsv_svs(const char **argv, uint16_t argc,
645 struct gnss_satellite *satellites, uint16_t size)
646 {
647 const struct gsv_header_args *header_args = (const struct gsv_header_args *)argv;
648 const struct gsv_sv_args *sv_args = (const struct gsv_sv_args *)(argv + 4);
649 uint16_t sv_args_size;
650 enum gnss_system sv_system;
651
652 __ASSERT(argv != NULL, "argv argument must be provided");
653 __ASSERT(satellites != NULL, "satellites argument must be provided");
654
655 if (argc < 9) {
656 return 0;
657 }
658
659 sv_args_size = (argc - GNSS_NMEA0183_GSV_HDR_ARG_CNT) / GNSS_NMEA0183_GSV_SV_ARG_CNT;
660
661 if (size < sv_args_size) {
662 return -ENOMEM;
663 }
664
665 if (parse_gsv_svs(satellites, sv_args, sv_args_size) < 0) {
666 return -EINVAL;
667 }
668
669 if (gnss_system_from_gsv_header_args(header_args, &sv_system) < 0) {
670 return -EINVAL;
671 }
672
673 for (uint16_t i = 0; i < sv_args_size; i++) {
674 align_satellite_with_gnss_system(sv_system, &satellites[i]);
675 }
676
677 return (int)sv_args_size;
678 }
679
