1 /*
2 * This file is Copyright (c) 2010-2018 by the GPSD project
3 * SPDX-License-Identifier: BSD-2-clause
4 */
5
6 #include "gpsd_config.h" /* must be before all includes */
7
8 #include <math.h>
9 #include <stdlib.h> /* for labs() */
10 #include <time.h>
11
12 #include "gpsd.h"
13 #include "bits.h"
14 #include "gpsmon.h"
15
16 #ifdef UBLOX_ENABLE
17 #include "driver_ubx.h"
18 extern const struct gps_type_t driver_ubx;
19 static WINDOW *satwin, *navsolwin, *dopwin, *ppswin;
20
21 #define display (void)mvwprintw
22
ubx_initialize(void)23 static bool ubx_initialize(void)
24 {
25 int i;
26
27 /* "heavily inspired" by monitor_nmea.c */
28 if ((satwin = derwin(devicewin, 19, 28, 0, 0)) == NULL)
29 return false;
30 (void)wborder(satwin, 0, 0, 0, 0, 0, 0, 0, 0), (void)syncok(satwin, true);
31 (void)wattrset(satwin, A_BOLD);
32 display(satwin, 1, 1, "Ch PRN Az El S/N Flag U");
33 for (i = 0; i < 16; i++)
34 display(satwin, (int)(i + 2), 1, "%2d", i);
35 display(satwin, 18, 7, " NAV_SVINFO ");
36 (void)wattrset(satwin, A_NORMAL);
37
38 /* "heavily inspired" by monitor_nmea.c */
39 if ((navsolwin = derwin(devicewin, 13, 51, 0, 28)) == NULL)
40 return false;
41 (void)wborder(navsolwin, 0, 0, 0, 0, 0, 0, 0, 0),
42 (void)wattrset(navsolwin, A_BOLD);
43 (void)wmove(navsolwin, 1, 1);
44 (void)wprintw(navsolwin, "ECEF Pos:");
45 (void)wmove(navsolwin, 2, 1);
46 (void)wprintw(navsolwin, "ECEF Vel:");
47
48 (void)wmove(navsolwin, 4, 1);
49 (void)wprintw(navsolwin, "LTP Pos:");
50 (void)wmove(navsolwin, 5, 1);
51 (void)wprintw(navsolwin, "LTP Vel:");
52
53 (void)wmove(navsolwin, 7, 1);
54 (void)wprintw(navsolwin, "Time:");
55 (void)wmove(navsolwin, 8, 1);
56 (void)wprintw(navsolwin, "Time GPS: Day:");
57
58 (void)wmove(navsolwin, 10, 1);
59 (void)wprintw(navsolwin, "Est Pos Err m Est Vel Err m/s");
60 (void)wmove(navsolwin, 11, 1);
61 (void)wprintw(navsolwin, "PRNs: ## PDOP: xx.x Fix 0x.. Flags 0x..");
62
63 display(navsolwin, 12, 20, " NAV_SOL ");
64 (void)wattrset(navsolwin, A_NORMAL);
65
66 if ((dopwin = derwin(devicewin, 3, 51, 13, 28)) == NULL)
67 return false;
68 (void)wborder(dopwin, 0, 0, 0, 0, 0, 0, 0, 0);
69 (void)wattrset(dopwin, A_BOLD);
70 (void)wmove(dopwin, 1, 1);
71 (void)wprintw(dopwin, "DOP [H] [V] [P] [T] [G]");
72 display(dopwin, 2, 20, " NAV_DOP ");
73 (void)wattrset(dopwin, A_NORMAL);
74
75 if ((ppswin = derwin(devicewin, 3, 51, 16, 28)) == NULL)
76 return false;
77 (void)wborder(ppswin, 0, 0, 0, 0, 0, 0, 0, 0);
78 (void)syncok(ppswin, true);
79 (void)wattrset(ppswin, A_BOLD);
80 #define TOFF_LINE 1
81 #define TOFF_COLUMN 1
82 (void)mvwaddstr(ppswin, TOFF_LINE, TOFF_COLUMN, "TOFF: ");
83 (void)mvwaddstr(ppswin, TOFF_LINE, TOFF_COLUMN + 10, "N/A");
84 #define PPS_LINE 1
85 #define PPS_COLUMN 26
86 (void)mvwaddstr(ppswin, PPS_LINE, PPS_COLUMN, "PPS: ");
87 (void)mvwaddstr(ppswin, PPS_LINE, PPS_COLUMN + 10, "N/A");
88 (void)wattrset(ppswin, A_NORMAL);
89
90 return true;
91 }
92
display_nav_svinfo(unsigned char * buf,size_t data_len)93 static void display_nav_svinfo(unsigned char *buf, size_t data_len)
94 {
95 int i, nchan;
96
97 if (data_len < 152)
98 return;
99
100 nchan = (int)getub(buf, 4);
101 if (nchan > 16)
102 nchan = 16;
103
104 for (i = 0; i < nchan; i++) {
105 int off = 8 + 12 * i;
106 unsigned char ss, prn;
107 char el;
108 short az;
109 unsigned short fl;
110
111 prn = (unsigned char)getub(buf, off + 1);
112 fl = (unsigned short)getleu16(buf, off + 2);
113 ss = (unsigned char)getub(buf, off + 4);
114 el = (char)getsb(buf, off + 5);
115 az = (short)getles16(buf, off + 6);
116 (void)wmove(satwin, (int)(i + 2), 4);
117 (void)wprintw(satwin, "%3d %3d %3d %2d %04x %c",
118 prn, az, el, ss, fl, (fl & UBX_SAT_USED) ? 'Y' : ' ');
119 }
120 (void)wnoutrefresh(satwin);
121 return;
122 }
123
display_nav_sol(unsigned char * buf,size_t data_len)124 static void display_nav_sol(unsigned char *buf, size_t data_len)
125 {
126 unsigned short gw = 0;
127 unsigned int tow = 0, flags;
128 double epx, epy, epz, evx, evy, evz;
129 unsigned char navmode;
130 struct gps_data_t g;
131
132 if (data_len != 52)
133 return;
134
135 navmode = (unsigned char)getub(buf, 10);
136 flags = (unsigned int)getub(buf, 11);
137
138 if ((flags & (UBX_SOL_VALID_WEEK | UBX_SOL_VALID_TIME)) != 0) {
139 tow = (unsigned int)getleu32(buf, 0);
140 gw = (unsigned short)getles16(buf, 8);
141 }
142
143 epx = (double)(getles32(buf, 12) / 100.0);
144 epy = (double)(getles32(buf, 16) / 100.0);
145 epz = (double)(getles32(buf, 20) / 100.0);
146 evx = (double)(getles32(buf, 28) / 100.0);
147 evy = (double)(getles32(buf, 32) / 100.0);
148 evz = (double)(getles32(buf, 36) / 100.0);
149 (void)ecef_to_wgs84fix(&g.fix, epx, epy, epz, evx, evy, evz);
150 /* maybe should check the ecef_to_wgs84fix() return code? */
151
152 g.fix.epx = g.fix.epy = (double)(getles32(buf, 24) / 100.0);
153 g.fix.eps = (double)(getles32(buf, 40) / 100.0);
154 g.dop.pdop = (double)(getleu16(buf, 44) / 100.0);
155 g.satellites_used = (int)getub(buf, 47);
156
157 (void)wmove(navsolwin, 1, 11);
158 (void)wprintw(navsolwin, "%+10.2fm %+10.2fm %+10.2fm", epx, epy, epz);
159 (void)wmove(navsolwin, 2, 11);
160 (void)wprintw(navsolwin, "%+9.2fm/s %+9.2fm/s %+9.2fm/s", evx, evy, evz);
161
162 (void)wmove(navsolwin, 4, 11);
163 (void)wattrset(navsolwin, A_UNDERLINE);
164 (void)wprintw(navsolwin, "%12.9f %13.9f %8.2fm",
165 g.fix.latitude, g.fix.longitude, g.fix.altHAE);
166 (void)mvwaddch(navsolwin, 4, 23, ACS_DEGREE);
167 (void)mvwaddch(navsolwin, 4, 38, ACS_DEGREE);
168 (void)wmove(navsolwin, 5, 11);
169 (void)wprintw(navsolwin, "%6.2fm/s %5.1fo %6.2fm/s",
170 g.fix.speed, g.fix.track, g.fix.climb);
171 (void)mvwaddch(navsolwin, 5, 26, ACS_DEGREE);
172 (void)wattrset(navsolwin, A_NORMAL);
173
174 (void)wmove(navsolwin, 7, 7);
175 {
176 unsigned int day = tow / 86400000;
177 unsigned int tod = tow % 86400000;
178 unsigned int h = tod / 3600000;
179 unsigned int m = tod % 3600000;
180 unsigned int s = m % 60000;
181
182 m = (m - s) / 60000;
183
184 (void)wattrset(navsolwin, A_UNDERLINE);
185 (void)wprintw(navsolwin, "%u %02u:%02u:%05.2f", day, h, m, (double)s / 1000);
186 (void)wattrset(navsolwin, A_NORMAL);
187 }
188 (void)wmove(navsolwin, 8, 11);
189 if ((flags & (UBX_SOL_VALID_WEEK | UBX_SOL_VALID_TIME)) != 0) {
190 (void)wprintw(navsolwin, "%d+%10.3lf", gw, (double)(tow / 1000.0));
191 (void)wmove(navsolwin, 8, 36);
192 (void)wprintw(navsolwin, "%d", (tow / 86400000));
193 }
194
195 /* relies on the fact that epx and epy are set to same value */
196 (void)wmove(navsolwin, 10, 12);
197 (void)wprintw(navsolwin, "%7.2f", g.fix.epx);
198 (void)wmove(navsolwin, 10, 33);
199 (void)wprintw(navsolwin, "%6.2f", g.fix.epv);
200 (void)wmove(navsolwin, 11, 7);
201 (void)wprintw(navsolwin, "%2d", g.satellites_used);
202 (void)wmove(navsolwin, 11, 15);
203 (void)wprintw(navsolwin, "%5.1f", g.dop.pdop);
204 (void)wmove(navsolwin, 11, 25);
205 (void)wprintw(navsolwin, "0x%02x", navmode);
206 (void)wmove(navsolwin, 11, 36);
207 (void)wprintw(navsolwin, "0x%02x", flags);
208 (void)wnoutrefresh(navsolwin);
209 }
210
211
display_nav_dop(unsigned char * buf,size_t data_len)212 static void display_nav_dop(unsigned char *buf, size_t data_len)
213 {
214 if (data_len != 18)
215 return;
216 (void)wmove(dopwin, 1, 9);
217 (void)wprintw(dopwin, "%4.1f", getleu16(buf, 12) / 100.0);
218 (void)wmove(dopwin, 1, 18);
219 (void)wprintw(dopwin, "%4.1f", getleu16(buf, 10) / 100.0);
220 (void)wmove(dopwin, 1, 27);
221 (void)wprintw(dopwin, "%4.1f", getleu16(buf, 6) / 100.0);
222 (void)wmove(dopwin, 1, 36);
223 (void)wprintw(dopwin, "%4.1f", getleu16(buf, 8) / 100.0);
224 (void)wmove(dopwin, 1, 45);
225 (void)wprintw(dopwin, "%4.1f", getleu16(buf, 4) / 100.0);
226 (void)wnoutrefresh(dopwin);
227 }
228
ubx_update(void)229 static void ubx_update(void)
230 {
231 unsigned char *buf;
232 size_t data_len;
233 unsigned short msgid;
234
235 buf = session.lexer.outbuffer;
236 msgid = (unsigned short)((buf[2] << 8) | buf[3]);
237 data_len = (size_t) getles16(buf, 4);
238 switch (msgid) {
239 case UBX_NAV_SVINFO:
240 /* coverity_submit[tainted_data] */
241 display_nav_svinfo(&buf[6], data_len);
242 break;
243 case UBX_NAV_DOP:
244 display_nav_dop(&buf[6], data_len);
245 break;
246 case UBX_NAV_SOL:
247 display_nav_sol(&buf[6], data_len);
248 break;
249 default:
250 break;
251 }
252
253 toff_update(ppswin, TOFF_LINE, TOFF_COLUMN + 6);
254
255 pps_update(ppswin, PPS_LINE, PPS_COLUMN + 5);
256 }
257
ubx_command(char line[]UNUSED)258 static int ubx_command(char line[]UNUSED)
259 {
260 return COMMAND_UNKNOWN;
261 }
262
ubx_wrap(void)263 static void ubx_wrap(void)
264 {
265 (void)delwin(satwin);
266 return;
267 }
268
269 const struct monitor_object_t ubx_mmt = {
270 .initialize = ubx_initialize,
271 .update = ubx_update,
272 .command = ubx_command,
273 .wrap = ubx_wrap,
274 .min_y = 19,.min_x = 80, /* size of the device window */
275 .driver = &driver_ubx,
276 };
277 #endif
278