/* * This file is Copyright (c) 2010-2018 by the GPSD project * SPDX-License-Identifier: BSD-2-clause */ #include #include /* for labs() */ #include #include "gpsd.h" #include "bits.h" #include "gpsmon.h" #ifdef UBLOX_ENABLE #include "driver_ubx.h" extern const struct gps_type_t driver_ubx; static WINDOW *satwin, *navsolwin, *dopwin, *ppswin; #define display (void)mvwprintw static bool ubx_initialize(void) { int i; /* "heavily inspired" by monitor_nmea.c */ if ((satwin = derwin(devicewin, 19, 28, 0, 0)) == NULL) return false; (void)wborder(satwin, 0, 0, 0, 0, 0, 0, 0, 0), (void)syncok(satwin, true); (void)wattrset(satwin, A_BOLD); display(satwin, 1, 1, "Ch PRN Az El S/N Flag U"); for (i = 0; i < 16; i++) display(satwin, (int)(i + 2), 1, "%2d", i); display(satwin, 18, 7, " NAV_SVINFO "); (void)wattrset(satwin, A_NORMAL); /* "heavily inspired" by monitor_nmea.c */ if ((navsolwin = derwin(devicewin, 13, 51, 0, 28)) == NULL) return false; (void)wborder(navsolwin, 0, 0, 0, 0, 0, 0, 0, 0), (void)wattrset(navsolwin, A_BOLD); (void)wmove(navsolwin, 1, 1); (void)wprintw(navsolwin, "ECEF Pos:"); (void)wmove(navsolwin, 2, 1); (void)wprintw(navsolwin, "ECEF Vel:"); (void)wmove(navsolwin, 4, 1); (void)wprintw(navsolwin, "LTP Pos:"); (void)wmove(navsolwin, 5, 1); (void)wprintw(navsolwin, "LTP Vel:"); (void)wmove(navsolwin, 7, 1); (void)wprintw(navsolwin, "Time:"); (void)wmove(navsolwin, 8, 1); (void)wprintw(navsolwin, "Time GPS: Day:"); (void)wmove(navsolwin, 10, 1); (void)wprintw(navsolwin, "Est Pos Err m Est Vel Err m/s"); (void)wmove(navsolwin, 11, 1); (void)wprintw(navsolwin, "PRNs: ## PDOP: xx.x Fix 0x.. Flags 0x.."); display(navsolwin, 12, 20, " NAV_SOL "); (void)wattrset(navsolwin, A_NORMAL); if ((dopwin = derwin(devicewin, 3, 51, 13, 28)) == NULL) return false; (void)wborder(dopwin, 0, 0, 0, 0, 0, 0, 0, 0); (void)wattrset(dopwin, A_BOLD); (void)wmove(dopwin, 1, 1); (void)wprintw(dopwin, "DOP [H] [V] [P] [T] [G]"); display(dopwin, 2, 20, " NAV_DOP "); (void)wattrset(dopwin, A_NORMAL); if ((ppswin = derwin(devicewin, 3, 51, 16, 28)) == NULL) return false; (void)wborder(ppswin, 0, 0, 0, 0, 0, 0, 0, 0); (void)syncok(ppswin, true); (void)wattrset(ppswin, A_BOLD); #define TOFF_LINE 1 #define TOFF_COLUMN 1 (void)mvwaddstr(ppswin, TOFF_LINE, TOFF_COLUMN, "TOFF: "); #ifndef PPS_ENABLE (void)mvwaddstr(ppswin, TOFF_LINE, TOFF_COLUMN + 10, "N/A"); #endif /* PPS_ENABLE */ #define PPS_LINE 1 #define PPS_COLUMN 26 (void)mvwaddstr(ppswin, PPS_LINE, PPS_COLUMN, "PPS: "); #ifndef PPS_ENABLE (void)mvwaddstr(ppswin, PPS_LINE, PPS_COLUMN + 10, "N/A"); #endif /* PPS_ENABLE */ (void)wattrset(ppswin, A_NORMAL); return true; } static void display_nav_svinfo(unsigned char *buf, size_t data_len) { int i, nchan; if (data_len < 152) return; nchan = (int)getub(buf, 4); if (nchan > 16) nchan = 16; for (i = 0; i < nchan; i++) { int off = 8 + 12 * i; unsigned char ss, prn; char el; short az; unsigned short fl; prn = (unsigned char)getub(buf, off + 1); fl = (unsigned short)getleu16(buf, off + 2); ss = (unsigned char)getub(buf, off + 4); el = (char)getsb(buf, off + 5); az = (short)getles16(buf, off + 6); (void)wmove(satwin, (int)(i + 2), 4); (void)wprintw(satwin, "%3d %3d %3d %2d %04x %c", prn, az, el, ss, fl, (fl & UBX_SAT_USED) ? 'Y' : ' '); } (void)wnoutrefresh(satwin); return; } static void display_nav_sol(unsigned char *buf, size_t data_len) { unsigned short gw = 0; unsigned int tow = 0, flags; double epx, epy, epz, evx, evy, evz; unsigned char navmode; struct gps_data_t g; double separation; if (data_len != 52) return; navmode = (unsigned char)getub(buf, 10); flags = (unsigned int)getub(buf, 11); if ((flags & (UBX_SOL_VALID_WEEK | UBX_SOL_VALID_TIME)) != 0) { tow = (unsigned int)getleu32(buf, 0); gw = (unsigned short)getles16(buf, 8); } epx = (double)(getles32(buf, 12) / 100.0); epy = (double)(getles32(buf, 16) / 100.0); epz = (double)(getles32(buf, 20) / 100.0); evx = (double)(getles32(buf, 28) / 100.0); evy = (double)(getles32(buf, 32) / 100.0); evz = (double)(getles32(buf, 36) / 100.0); ecef_to_wgs84fix(&g.fix, &separation, epx, epy, epz, evx, evy, evz); g.fix.epx = g.fix.epy = (double)(getles32(buf, 24) / 100.0); g.fix.eps = (double)(getles32(buf, 40) / 100.0); g.dop.pdop = (double)(getleu16(buf, 44) / 100.0); g.satellites_used = (int)getub(buf, 47); (void)wmove(navsolwin, 1, 11); (void)wprintw(navsolwin, "%+10.2fm %+10.2fm %+10.2fm", epx, epy, epz); (void)wmove(navsolwin, 2, 11); (void)wprintw(navsolwin, "%+9.2fm/s %+9.2fm/s %+9.2fm/s", evx, evy, evz); (void)wmove(navsolwin, 4, 11); (void)wattrset(navsolwin, A_UNDERLINE); (void)wprintw(navsolwin, "%12.9f %13.9f %8.2fm", g.fix.latitude, g.fix.longitude, g.fix.altitude); (void)mvwaddch(navsolwin, 4, 23, ACS_DEGREE); (void)mvwaddch(navsolwin, 4, 38, ACS_DEGREE); (void)wmove(navsolwin, 5, 11); (void)wprintw(navsolwin, "%6.2fm/s %5.1fo %6.2fm/s", g.fix.speed, g.fix.track, g.fix.climb); (void)mvwaddch(navsolwin, 5, 26, ACS_DEGREE); (void)wattrset(navsolwin, A_NORMAL); (void)wmove(navsolwin, 7, 7); { unsigned int day = tow / 86400000; unsigned int tod = tow % 86400000; unsigned int h = tod / 3600000; unsigned int m = tod % 3600000; unsigned int s = m % 60000; m = (m - s) / 60000; (void)wattrset(navsolwin, A_UNDERLINE); (void)wprintw(navsolwin, "%u %02u:%02u:%05.2f", day, h, m, (double)s / 1000); (void)wattrset(navsolwin, A_NORMAL); } (void)wmove(navsolwin, 8, 11); if ((flags & (UBX_SOL_VALID_WEEK | UBX_SOL_VALID_TIME)) != 0) { (void)wprintw(navsolwin, "%d+%10.3lf", gw, (double)(tow / 1000.0)); (void)wmove(navsolwin, 8, 36); (void)wprintw(navsolwin, "%d", (tow / 86400000)); } /* relies on the fact that epx and epy are set to same value */ (void)wmove(navsolwin, 10, 12); (void)wprintw(navsolwin, "%7.2f", g.fix.epx); (void)wmove(navsolwin, 10, 33); (void)wprintw(navsolwin, "%6.2f", g.fix.epv); (void)wmove(navsolwin, 11, 7); (void)wprintw(navsolwin, "%2d", g.satellites_used); (void)wmove(navsolwin, 11, 15); (void)wprintw(navsolwin, "%5.1f", g.dop.pdop); (void)wmove(navsolwin, 11, 25); (void)wprintw(navsolwin, "0x%02x", navmode); (void)wmove(navsolwin, 11, 36); (void)wprintw(navsolwin, "0x%02x", flags); (void)wnoutrefresh(navsolwin); } static void display_nav_dop(unsigned char *buf, size_t data_len) { if (data_len != 18) return; (void)wmove(dopwin, 1, 9); (void)wprintw(dopwin, "%4.1f", getleu16(buf, 12) / 100.0); (void)wmove(dopwin, 1, 18); (void)wprintw(dopwin, "%4.1f", getleu16(buf, 10) / 100.0); (void)wmove(dopwin, 1, 27); (void)wprintw(dopwin, "%4.1f", getleu16(buf, 6) / 100.0); (void)wmove(dopwin, 1, 36); (void)wprintw(dopwin, "%4.1f", getleu16(buf, 8) / 100.0); (void)wmove(dopwin, 1, 45); (void)wprintw(dopwin, "%4.1f", getleu16(buf, 4) / 100.0); (void)wnoutrefresh(dopwin); } static void ubx_update(void) { unsigned char *buf; size_t data_len; unsigned short msgid; buf = session.lexer.outbuffer; msgid = (unsigned short)((buf[2] << 8) | buf[3]); data_len = (size_t) getles16(buf, 4); switch (msgid) { case UBX_NAV_SVINFO: /* coverity_submit[tainted_data] */ display_nav_svinfo(&buf[6], data_len); break; case UBX_NAV_DOP: display_nav_dop(&buf[6], data_len); break; case UBX_NAV_SOL: display_nav_sol(&buf[6], data_len); break; default: break; } #ifdef NTP_ENABLE toff_update(ppswin, TOFF_LINE, TOFF_COLUMN + 6); #endif /* NTP_ENABLE */ #ifdef PPS_ENABLE pps_update(ppswin, PPS_LINE, PPS_COLUMN + 5); #endif /* PPS_ENABLE */ } static int ubx_command(char line[]UNUSED) { return COMMAND_UNKNOWN; } static void ubx_wrap(void) { (void)delwin(satwin); return; } const struct monitor_object_t ubx_mmt = { .initialize = ubx_initialize, .update = ubx_update, .command = ubx_command, .wrap = ubx_wrap, .min_y = 19,.min_x = 80, /* size of the device window */ .driver = &driver_ubx, }; #endif