monitor_ubx.c

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