1 /*
2 * Copyright (c) 2023 Bjarki Arge Andreasen
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include "gnss_dump.h"
8 #include <stdlib.h>
9 #include <zephyr/kernel.h>
10 #include <zephyr/logging/log.h>
11
12 #include <string.h>
13
14 #if CONFIG_GNSS_DUMP_TO_LOG
15 static char dump_buf[CONFIG_GNSS_DUMP_TO_LOG_BUF_SIZE];
16 #endif /* CONFIG_GNSS_DUMP_TO_LOG */
17
gnss_fix_status_to_str(enum gnss_fix_status fix_status)18 static const char *gnss_fix_status_to_str(enum gnss_fix_status fix_status)
19 {
20 switch (fix_status) {
21 case GNSS_FIX_STATUS_NO_FIX:
22 return "NO_FIX";
23 case GNSS_FIX_STATUS_GNSS_FIX:
24 return "GNSS_FIX";
25 case GNSS_FIX_STATUS_DGNSS_FIX:
26 return "DGNSS_FIX";
27 case GNSS_FIX_STATUS_ESTIMATED_FIX:
28 return "ESTIMATED_FIX";
29 }
30
31 return "unknown";
32 }
33
gnss_fix_quality_to_str(enum gnss_fix_quality fix_quality)34 static const char *gnss_fix_quality_to_str(enum gnss_fix_quality fix_quality)
35 {
36 switch (fix_quality) {
37 case GNSS_FIX_QUALITY_INVALID:
38 return "INVALID";
39 case GNSS_FIX_QUALITY_GNSS_SPS:
40 return "GNSS_SPS";
41 case GNSS_FIX_QUALITY_DGNSS:
42 return "DGNSS";
43 case GNSS_FIX_QUALITY_GNSS_PPS:
44 return "GNSS_PPS";
45 case GNSS_FIX_QUALITY_RTK:
46 return "RTK";
47 case GNSS_FIX_QUALITY_FLOAT_RTK:
48 return "FLOAT_RTK";
49 case GNSS_FIX_QUALITY_ESTIMATED:
50 return "ESTIMATED";
51 }
52
53 return "unknown";
54 }
55
56 #if CONFIG_GNSS_SATELLITES
gnss_system_to_str(enum gnss_system system)57 static const char *gnss_system_to_str(enum gnss_system system)
58 {
59 switch (system) {
60 case GNSS_SYSTEM_GPS:
61 return "GPS";
62 case GNSS_SYSTEM_GLONASS:
63 return "GLONASS";
64 case GNSS_SYSTEM_GALILEO:
65 return "GALILEO";
66 case GNSS_SYSTEM_BEIDOU:
67 return "BEIDOU";
68 case GNSS_SYSTEM_QZSS:
69 return "QZSS";
70 case GNSS_SYSTEM_IRNSS:
71 return "IRNSS";
72 case GNSS_SYSTEM_SBAS:
73 return "SBAS";
74 case GNSS_SYSTEM_IMES:
75 return "IMES";
76 }
77
78 return "unknown";
79 }
80 #endif
81
gnss_dump_info(char * str,uint16_t strsize,const struct gnss_info * info)82 int gnss_dump_info(char *str, uint16_t strsize, const struct gnss_info *info)
83 {
84 int ret;
85 const char *fmt = "gnss_info: {satellites_cnt: %u, hdop: %u.%u, fix_status: %s, "
86 "fix_quality: %s}";
87
88 ret = snprintk(str, strsize, fmt, info->satellites_cnt, info->hdop / 1000,
89 info->hdop % 1000, gnss_fix_status_to_str(info->fix_status),
90 gnss_fix_quality_to_str(info->fix_quality));
91
92 return (strsize < ret) ? -ENOMEM : 0;
93 }
94
gnss_dump_nav_data(char * str,uint16_t strsize,const struct navigation_data * nav_data)95 int gnss_dump_nav_data(char *str, uint16_t strsize, const struct navigation_data *nav_data)
96 {
97 int ret;
98 const char *fmt = "navigation_data: {latitude: %s%lli.%09lli, longitude : %s%lli.%09lli, "
99 "bearing %u.%03u, speed %u.%03u, altitude: %s%i.%03i}";
100 char *lat_sign = nav_data->latitude < 0 ? "-" : "";
101 char *lon_sign = nav_data->longitude < 0 ? "-" : "";
102 char *alt_sign = nav_data->altitude < 0 ? "-" : "";
103
104 ret = snprintk(str, strsize, fmt,
105 lat_sign,
106 llabs(nav_data->latitude) / 1000000000,
107 llabs(nav_data->latitude) % 1000000000,
108 lon_sign,
109 llabs(nav_data->longitude) / 1000000000,
110 llabs(nav_data->longitude) % 1000000000,
111 nav_data->bearing / 1000, nav_data->bearing % 1000,
112 nav_data->speed / 1000, nav_data->speed % 1000,
113 alt_sign, abs(nav_data->altitude) / 1000, abs(nav_data->altitude) % 1000);
114
115 return (strsize < ret) ? -ENOMEM : 0;
116 }
117
gnss_dump_time(char * str,uint16_t strsize,const struct gnss_time * utc)118 int gnss_dump_time(char *str, uint16_t strsize, const struct gnss_time *utc)
119 {
120 int ret;
121 const char *fmt = "gnss_time: {hour: %u, minute: %u, millisecond %u, month_day %u, "
122 "month: %u, century_year: %u}";
123
124 ret = snprintk(str, strsize, fmt, utc->hour, utc->minute, utc->millisecond,
125 utc->month_day, utc->month, utc->century_year);
126
127 return (strsize < ret) ? -ENOMEM : 0;
128 }
129
130 #if CONFIG_GNSS_SATELLITES
gnss_dump_satellite(char * str,uint16_t strsize,const struct gnss_satellite * satellite)131 int gnss_dump_satellite(char *str, uint16_t strsize, const struct gnss_satellite *satellite)
132 {
133 int ret;
134 const char *fmt = "gnss_satellite: {prn: %u, snr: %u, elevation %u, azimuth %u, "
135 "system: %s, is_tracked: %u}";
136
137 ret = snprintk(str, strsize, fmt, satellite->prn, satellite->snr, satellite->elevation,
138 satellite->azimuth, gnss_system_to_str(satellite->system),
139 satellite->is_tracked);
140
141 return (strsize < ret) ? -ENOMEM : 0;
142 }
143 #endif
144
145 #if CONFIG_GNSS_DUMP_TO_LOG
gnss_dump_data_to_log(const struct device * dev,const struct gnss_data * data)146 static void gnss_dump_data_to_log(const struct device *dev, const struct gnss_data *data)
147 {
148 if (gnss_dump_info(dump_buf, sizeof(dump_buf), &data->info) < 0) {
149 return;
150 }
151
152 LOG_PRINTK("%s: %s\r\n", dev->name, dump_buf);
153
154 if (gnss_dump_nav_data(dump_buf, sizeof(dump_buf), &data->nav_data) < 0) {
155 return;
156 }
157
158 LOG_PRINTK("%s: %s\r\n", dev->name, dump_buf);
159
160 if (gnss_dump_time(dump_buf, sizeof(dump_buf), &data->utc) < 0) {
161 return;
162 }
163
164 LOG_PRINTK("%s: %s\r\n", dev->name, dump_buf);
165 }
166
167 GNSS_DATA_CALLBACK_DEFINE(NULL, gnss_dump_data_to_log);
168 #endif
169
170 #if defined(CONFIG_GNSS_DUMP_TO_LOG) && defined(CONFIG_GNSS_SATELLITES)
gnss_dump_satellites_to_log(const struct device * dev,const struct gnss_satellite * satellites,uint16_t size)171 static void gnss_dump_satellites_to_log(const struct device *dev,
172 const struct gnss_satellite *satellites, uint16_t size)
173 {
174 for (uint16_t i = 0; i < size; i++) {
175 if (gnss_dump_satellite(dump_buf, sizeof(dump_buf), &satellites[i]) < 0) {
176 return;
177 }
178
179 LOG_PRINTK("%s: %s\r\n", dev->name, dump_buf);
180 }
181 }
182
183 GNSS_SATELLITES_CALLBACK_DEFINE(NULL, gnss_dump_satellites_to_log);
184 #endif
185
