/*

* This file is Copyright (c) 2010 by the GPSD project

* BSD terms apply: see the file COPYING in the distribution root for details.

*/

#include <math.h>

#include <time.h>

#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;

/*@ -onlytrans @*/

/* "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);

/*@ -onlytrans @*/

/* "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)wattrset(ppswin, A_BOLD);

(void)mvwaddstr(ppswin, 1, 1, "PPS offset: ");

#ifndef PPS_ENABLE

(void)mvwaddstr(ppswin, 1, 13, "Not available");

#endif /* PPS_ENABLE */

display(ppswin, 2, 22, " PPS ");

(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;

}

/*@ -mustfreeonly -compdestroy @*/

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;

#ifdef S_SPLINT_S

assert(navsolwin != NULL);

#endif /* S_SPLINT_S */

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);

/*@ -compdef @*/

{

unsigned int day = tow / 8640000;

unsigned int tod = tow % 8640000;

unsigned int h = tod / 360000;

unsigned int m = tod % 360000;

unsigned int s = m % 6000;

m = (m - s) / 6000;

(void)wattrset(navsolwin, A_UNDERLINE);

(void)wprintw(navsolwin, "%u %02u:%02u:%05.2f", day, h, m, (double)s / 100);

(void)wattrset(navsolwin, A_NORMAL);

}

/*@ +compdef @*/

(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);

}

/*@ +mustfreeonly +compdestroy @*/

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;

#ifdef PPS_ENABLE

struct timedrift_t drift;

#endif /* PPS_ENABLE */

buf = session.packet.outbuffer;

msgid = (unsigned short)((buf[2] << 8) | buf[3]);

data_len = (size_t) getles16(buf, 4);

switch (msgid) {

case UBX_NAV_SVINFO:

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 PPS_ENABLE

/*@-compdef@*/

/*@-type@*/ /* splint is confused about struct timespec */

if (pps_thread_lastpps(&session, &drift) > 0) {

double timedelta = timespec_diff_ns(drift.real, drift.clock) * 1e-9;

(void)mvwprintw(ppswin, 1, 13, "%.9f", timedelta);

(void)wnoutrefresh(ppswin);

}

/*@+type@*/

/*@+compdef@*/

#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