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 <errno.h>
11 #include <stdlib.h>
12
13 #include "gnss_parse.h"
14
15 #define GNSS_PARSE_NANO_KNOTS_IN_MMS (1943840LL)
16 #define GNSS_PARSE_NANO (1000000000LL)
17 #define GNSS_PARSE_MICRO (1000000LL)
18 #define GNSS_PARSE_MILLI (1000LL)
19
gnss_parse_dec_to_nano(const char * str,int64_t * nano)20 int gnss_parse_dec_to_nano(const char *str, int64_t *nano)
21 {
22 int64_t sum = 0;
23 int8_t decimal = -1;
24 int8_t pos = 0;
25 int8_t start = 0;
26 int64_t increment;
27
28 __ASSERT(str != NULL, "str argument must be provided");
29 __ASSERT(nano != NULL, "nano argument must be provided");
30
31 /* Find decimal */
32 while (str[pos] != '\0') {
33 /* Verify if char is decimal */
34 if (str[pos] == '.') {
35 decimal = pos;
36 break;
37 }
38
39 /* Advance position */
40 pos++;
41 }
42
43 /* Determine starting position based on decimal location */
44 pos = decimal < 0 ? pos - 1 : decimal - 1;
45
46 /* Skip sign if it exists */
47 start = str[0] == '-' ? 1 : 0;
48
49 /* Add whole value to sum */
50 increment = GNSS_PARSE_NANO;
51 while (start <= pos) {
52 /* Verify char is decimal */
53 if (str[pos] < '0' || str[pos] > '9') {
54 return -EINVAL;
55 }
56
57 /* Add value to sum */
58 sum += (str[pos] - '0') * increment;
59
60 /* Update increment */
61 increment *= 10;
62
63 /* Degrement position */
64 pos--;
65 }
66
67 /* Check if decimal was found */
68 if (decimal < 0) {
69 /* Set sign of sum */
70 sum = start == 1 ? -sum : sum;
71
72 *nano = sum;
73 return 0;
74 }
75
76 /* Convert decimal part to nano fractions and add it to sum */
77 pos = decimal + 1;
78 increment = GNSS_PARSE_NANO / 10LL;
79 while (str[pos] != '\0') {
80 /* Verify char is decimal */
81 if (str[pos] < '0' || str[pos] > '9') {
82 return -EINVAL;
83 }
84
85 /* Add value to micro_degrees */
86 sum += (str[pos] - '0') * increment;
87
88 /* Update unit */
89 increment /= 10;
90
91 /* Increment position */
92 pos++;
93 }
94
95 /* Set sign of sum */
96 sum = start == 1 ? -sum : sum;
97
98 *nano = sum;
99 return 0;
100 }
101
gnss_parse_dec_to_micro(const char * str,uint64_t * micro)102 int gnss_parse_dec_to_micro(const char *str, uint64_t *micro)
103 {
104 int ret;
105
106 __ASSERT(str != NULL, "str argument must be provided");
107 __ASSERT(micro != NULL, "micro argument must be provided");
108
109 ret = gnss_parse_dec_to_nano(str, micro);
110 if (ret < 0) {
111 return ret;
112 }
113
114 *micro = (*micro) / GNSS_PARSE_MILLI;
115 return 0;
116 }
117
118
gnss_parse_dec_to_milli(const char * str,int64_t * milli)119 int gnss_parse_dec_to_milli(const char *str, int64_t *milli)
120 {
121 int ret;
122
123 __ASSERT(str != NULL, "str argument must be provided");
124 __ASSERT(milli != NULL, "milli argument must be provided");
125
126 ret = gnss_parse_dec_to_nano(str, milli);
127 if (ret < 0) {
128 return ret;
129 }
130
131 (*milli) = (*milli) / GNSS_PARSE_MICRO;
132 return 0;
133 }
134
gnss_parse_atoi(const char * str,uint8_t base,int32_t * integer)135 int gnss_parse_atoi(const char *str, uint8_t base, int32_t *integer)
136 {
137 char *end;
138
139 __ASSERT(str != NULL, "str argument must be provided");
140 __ASSERT(integer != NULL, "integer argument must be provided");
141
142 *integer = (int32_t)strtol(str, &end, (int)base);
143
144 if ('\0' != (*end)) {
145 return -EINVAL;
146 }
147
148 return 0;
149 }
150
