diff -Naur A/cgps.c C/cgps.c --- A/cgps.c 2019-06-08 17:48:57.651643221 +0530 +++ C/cgps.c 2019-07-03 13:11:36.584325207 +0530 @@ -597,6 +597,8 @@ (void)snprintf(scr, sizeof(scr), "n/a"); (void)mvwprintw(datawin, 13, DATAWIN_VALUE_OFFSET + 5, "%-*s", 22, scr); + + /* Fill in the time offset. */ if (isnan(gpsdata->fix.time) == 0) (void)snprintf(scr, sizeof(scr), "%.3f", diff -Naur A/driver_nmea0183.c C/driver_nmea0183.c --- A/driver_nmea0183.c 2019-06-08 17:48:57.651643221 +0530 +++ C/driver_nmea0183.c 2019-07-03 13:11:36.588325233 +0530 @@ -679,6 +679,9 @@ sp->azimuth = (short)atoi(field[fldnum++]); sp->ss = (float)atoi(field[fldnum++]); sp->used = false; + gpsd_log(&session->context->errout, LOG_INF, + "sp->PRN [%d]!\n", + sp->PRN); if (sp->PRN > 0) for (n = 0; n < MAXCHANNELS; n++) if (session->nmea.sats_used[n] == (unsigned short)sp->PRN) { diff -Naur A/driver_ubx.c C/driver_ubx.c --- A/driver_ubx.c 2019-06-08 17:48:57.651643221 +0530 +++ C/driver_ubx.c 2019-07-03 14:10:11.444151662 +0530 @@ -73,6 +73,8 @@ unsigned char *buf, size_t data_len); static void ubx_msg_mon_ver(struct gps_device_t *session, unsigned char *buf, size_t data_len); +static gps_mask_t ubx_msg_nav_velned(struct gps_device_t *session, unsigned char *buf, size_t data_len UNUSED); + #ifdef RECONFIGURE_ENABLE static void ubx_mode(struct gps_device_t *session, int mode); #endif /* RECONFIGURE_ENABLE */ @@ -195,6 +197,29 @@ } session->newdata.eps = (double)(getles32(buf, 40) / 100.0); + if(session->driver.ubx.last_serr > 0.0) { + gpsd_log(&session->context->errout, LOG_INF, "Modifying sAcc from VELNED \r\n"); + session->newdata.eps = session->driver.ubx.last_serr; + session->driver.ubx.last_serr = 0.0; + } + + if(session->driver.ubx.last_velN != 0.0) { + session->gpsdata.cust_param.velN = session->driver.ubx.last_velN; + gpsd_log(&session->context->errout, LOG_INF, "Modifying velN from VELNED:%lf \r\n", session->gpsdata.cust_param.velN); + session->driver.ubx.last_velN = 0.0; + } + + if(session->driver.ubx.last_velE != 0.0) { + session->gpsdata.cust_param.velE = session->driver.ubx.last_velE; + gpsd_log(&session->context->errout, LOG_INF, "Modifying velE from VELNED :%lf \r\n", session->gpsdata.cust_param.velE); + session->driver.ubx.last_velE = 0.0; + } + + if(session->driver.ubx.last_height != 0.0) { + session->gpsdata.cust_param.height = session->driver.ubx.last_height; + gpsd_log(&session->context->errout, LOG_INF, "Modifying height from VELNED :%lf\r\n", session->gpsdata.cust_param.height); + session->driver.ubx.last_height = 0.0; + } mask |= SPEEDERR_SET; /* Better to have a single point of truth about DOPs */ @@ -222,8 +247,8 @@ session->gpsdata.status = STATUS_FIX; mask |= MODE_SET | STATUS_SET; - gpsd_log(&session->context->errout, LOG_DATA, - "NAVSOL: time=%.2f lat=%.2f lon=%.2f alt=%.2f track=%.2f speed=%.2f climb=%.2f mode=%d status=%d used=%d\n", + gpsd_log(&session->context->errout, LOG_INF, + "*****NAVSOL: time=%.2f lat=%.2f lon=%.2f alt=%.2f track=%.2f speed=%.2f climb=%.2f mode=%d status=%d used=%d\n", session->newdata.time, session->newdata.latitude, session->newdata.longitude, @@ -246,9 +271,27 @@ { session->driver.ubx.last_herr = (double)(getleu32(buf, 20) / 1000.0); session->driver.ubx.last_verr = (double)(getleu32(buf, 24) / 1000.0); + session->driver.ubx.last_height = (double)(getleu32(buf, 12) / 1000.0); + gpsd_log(&session->context->errout, LOG_DATA, "POSLLH hAcc:%lf,vAcc:%lf height = %lf\r\n",session->driver.ubx.last_herr,session->driver.ubx.last_verr,session->driver.ubx.last_height); return 0; } + + /** + * Velocity solution in NED + */ +static gps_mask_t +ubx_msg_nav_velned(struct gps_device_t *session, unsigned char *buf, + size_t data_len UNUSED) +{ + session->driver.ubx.last_serr = (double)(getles32(buf, 28) / 100.0); + session->driver.ubx.last_velN = (double)(getles32(buf, 4) / 100.0); + session->driver.ubx.last_velE = (double)(getles32(buf, 8) / 100.0); + gpsd_log(&session->context->errout, LOG_DATA, "VELNED eps:%lf velN = %lf velE= %lf\r\n",session->driver.ubx.last_serr,session->driver.ubx.last_velN,session->driver.ubx.last_velE); + return 0; +} + + /** * Dilution of precision message */ @@ -488,6 +531,7 @@ break; case UBX_NAV_VELNED: gpsd_log(&session->context->errout, LOG_DATA, "UBX_NAV_VELNED\n"); + ubx_msg_nav_velned(session, &buf[UBX_PREFIX_LEN],data_len); break; case UBX_NAV_TIMEGPS: gpsd_log(&session->context->errout, LOG_PROG, "UBX_NAV_TIMEGPS\n"); @@ -930,6 +974,16 @@ /* request SW and HW Versions */ (void)ubx_write(session, UBX_CLASS_MON, 0x04, msg, 0); + msg[0] = 0x01; /* class */ + msg[1] = 0x02; /* msg id = NAV-POSLLH */ + msg[2] = 0x01; /* rate */ + (void)ubx_write(session, 0x06u, 0x01, msg, 3); + + msg[0] = 0x01; /* class */ + msg[1] = 0x012; /* msg id = NAV-VELNED */ + msg[2] = 0x01; /* rate */ + (void)ubx_write(session, 0x06u, 0x01, msg, 3); + msg[0] = 0x01; /* class */ msg[1] = 0x04; /* msg id = UBX_NAV_DOP */ msg[2] = 0x01; /* rate */ diff -Naur A/gps/gps.py C/gps/gps.py --- A/gps/gps.py 2019-06-08 17:48:57.667643222 +0530 +++ C/gps/gps.py 2019-07-03 14:17:18.034507450 +0530 @@ -95,7 +95,7 @@ self.epd = NaN self.eps = NaN self.epc = NaN - + class gpsdata: "Position, track, velocity and status information returned by a GPS." @@ -226,8 +226,8 @@ self.fix.epd = default(11) self.fix.eps = default(12, SPEEDERR_SET) self.fix.epc = default(13, CLIMBERR_SET) - if len(fields) > 14: - self.fix.mode = default(14, MODE_SET, int) + if len(fields) > 17: + self.fix.mode = default(17, MODE_SET, int) else: if self.valid & ALTITUDE_SET: self.fix.mode = MODE_2D diff -Naur A/gpsd.c C/gpsd.c --- A/gpsd.c 2019-06-08 17:48:57.655643221 +0530 +++ C/gpsd.c 2019-07-03 19:23:31.241951385 +0530 @@ -571,7 +571,7 @@ if (context.errout.debug >= LOG_CLIENT) { if (isprint((unsigned char) buf[0])) gpsd_log(&context.errout, LOG_CLIENT, - "=> client(%d): %s\n", sub_index(sub), buf); + "****=> client(%d): %s\n", sub_index(sub), buf); else { #ifndef __clang_analyzer__ char *cp, buf2[MAX_PACKET_LENGTH * 3]; @@ -580,7 +580,7 @@ str_appendf(buf2, sizeof(buf2), "%02x", (unsigned int)(*cp & 0xff)); gpsd_log(&context.errout, LOG_CLIENT, - "=> client(%d): =%s\n", sub_index(sub), buf2); + "###=> client(%d): =%s\n", sub_index(sub), buf2); #endif /* __clang_analyzer__ */ } } @@ -594,26 +594,30 @@ #endif /* PPS_ENABLE */ if (status == (ssize_t) len) - return status; + { + return status; + } else if (status > -1) { - gpsd_log(&context.errout, LOG_INF, - "short write disconnecting client(%d)\n", - sub_index(sub)); - detach_client(sub); - return 0; + gpsd_log(&context.errout, LOG_INF, + "short write disconnecting client(%d)\n", + sub_index(sub)); + detach_client(sub); + return 0; } else if (errno == EAGAIN || errno == EINTR) - return 0; /* no data written, and errno says to retry */ + { + return 0; /* no data written, and errno says to retry */ + } else if (errno == EBADF) - gpsd_log(&context.errout, LOG_WARN, - "client(%d) has vanished.\n", sub_index(sub)); + gpsd_log(&context.errout, LOG_INF, + "client(%d) has vanished.\n", sub_index(sub)); else if (errno == EWOULDBLOCK - && time(NULL) - sub->active > NOREAD_TIMEOUT) - gpsd_log(&context.errout, LOG_INF, - "client(%d) timed out.\n", sub_index(sub)); + && time(NULL) - sub->active > NOREAD_TIMEOUT) + gpsd_log(&context.errout, LOG_INF, + "client(%d) timed out.\n", sub_index(sub)); else - gpsd_log(&context.errout, LOG_INF, - "client(%d) write: %s\n", - sub_index(sub), strerror(errno)); + gpsd_log(&context.errout, LOG_INF, + "client(%d) write: %s\n", + sub_index(sub), strerror(errno)); detach_client(sub); return status; } @@ -623,6 +627,8 @@ const char *sentence, ...) /* notify all JSON-watching clients of a given device about an event */ { + gpsd_log(&context.errout, LOG_INF, + "notify_watchers\n"); va_list ap; char buf[BUFSIZ]; struct subscriber_t *sub; @@ -1643,7 +1649,9 @@ device, &sub->policy, buf, sizeof(buf)); if (buf[0] != '\0') + { (void)throttled_write(sub, buf, strlen(buf)); + } } } @@ -2196,7 +2204,7 @@ accept(msocks[i], (struct sockaddr *)&fsin, &alen); if (BAD_SOCKET(ssock)) - gpsd_log(&context.errout, LOG_ERROR, + gpsd_log(&context.errout, LOG_INF, "accept: %s\n", strerror(errno)); else { struct subscriber_t *client = NULL; @@ -2210,7 +2218,7 @@ c_ip = netlib_sock2ip(ssock); client = allocate_client(); if (client == NULL) { - gpsd_log(&context.errout, LOG_ERROR, + gpsd_log(&context.errout, LOG_INF, "Client %s connect on fd %d -" "no subscriber slots available\n", c_ip, ssock); @@ -2228,7 +2236,7 @@ adjust_max_fd(ssock, true); client->fd = ssock; client->active = time(NULL); - gpsd_log(&context.errout, LOG_SPIN, + gpsd_log(&context.errout, LOG_INF, "client %s (%d) connect on fd %d\n", c_ip, sub_index(client), ssock); json_version_dump(announce, sizeof(announce)); @@ -2290,15 +2298,19 @@ device, all_reports, DEVICE_REAWAKE)) { case DEVICE_READY: + gpsd_log(&context.errout, LOG_INF,"DEVICE_READY\n"); FD_SET(device->gpsdata.gps_fd, &all_fds); adjust_max_fd(device->gpsdata.gps_fd, true); break; case DEVICE_UNREADY: + gpsd_log(&context.errout, LOG_INF,"DEVICE_UNREADY\n"); FD_CLR(device->gpsdata.gps_fd, &all_fds); adjust_max_fd(device->gpsdata.gps_fd, false); break; case DEVICE_ERROR: + gpsd_log(&context.errout, LOG_INF,"DEVICE_ERROR\n"); case DEVICE_EOF: + gpsd_log(&context.errout, LOG_INF,"DEVICE_EOF\n"); deactivate_device(device); break; default: @@ -2323,7 +2335,9 @@ /* accept and execute commands for all clients */ for (sub = subscribers; sub < subscribers + MAX_CLIENTS; sub++) { if (sub->active == 0) + { continue; + } lock_subscriber(sub); if (FD_ISSET(sub->fd, &rfds)) { @@ -2332,13 +2346,15 @@ unlock_subscriber(sub); - gpsd_log(&context.errout, LOG_PROG, + gpsd_log(&context.errout, LOG_INF, "checking client(%d)\n", sub_index(sub)); if ((buflen = (int)recv(sub->fd, buf, sizeof(buf) - 1, 0)) <= 0) { + gpsd_log(&context.errout, LOG_INF, "recieved < = 0 \n"); detach_client(sub); } else { + gpsd_log(&context.errout, LOG_INF, "***recvd buflen = %d\n", buflen); if (buf[buflen - 1] != '\n') buf[buflen++] = '\n'; buf[buflen] = '\0'; @@ -2353,14 +2369,17 @@ */ sub->active = time(NULL); if (handle_gpsd_request(sub, buf) < 0) + { + gpsd_log(&context.errout, LOG_INF, "***handle_gpsd_request failure..\r\n"); detach_client(sub); + } } } else { unlock_subscriber(sub); if (!sub->policy.watcher && time(NULL) - sub->active > COMMAND_TIMEOUT) { - gpsd_log(&context.errout, LOG_WARN, + gpsd_log(&context.errout, LOG_INF, "client(%d) timed out on command wait.\n", sub_index(sub)); detach_client(sub); diff -Naur A/gpsd.h-tail C/gpsd.h-tail --- A/gpsd.h-tail 2019-06-08 17:48:57.655643221 +0530 +++ C/gpsd.h-tail 2019-07-03 13:11:36.588325233 +0530 @@ -621,6 +621,10 @@ */ double last_herr; double last_verr; + double last_serr; + double last_velN; /*North component of velocity vector*/ + double last_velE; /*East velocity component*/ + double last_height; /*Height above ellipsoid*/ } ubx; #endif /* UBLOX_ENABLE */ #ifdef NAVCOM_ENABLE @@ -937,7 +941,7 @@ #define LIBGPS_DEBUG #endif /* CLIENTDEBUG_ENABLE */ #ifdef LIBGPS_DEBUG -#define DEBUG_CALLS 1 /* shallowest debug level */ +#define DEBUG_CALLS 8 /* shallowest debug level */ #define DEBUG_JSON 5 /* minimum level for verbose JSON debugging */ # define libgps_debug_trace(args) (void) libgps_trace args extern int libgps_debuglevel; diff -Naur A/gpsd_json.c C/gpsd_json.c --- A/gpsd_json.c 2019-06-08 17:48:57.655643221 +0530 +++ C/gpsd_json.c 2019-07-03 19:38:51.055799583 +0530 @@ -178,6 +178,16 @@ str_appendf(reply, replylen, "\"eps\":%.2f,", gpsdata->fix.eps); if ((gpsdata->fix.mode >= MODE_3D) && isnan(gpsdata->fix.epc) == 0) str_appendf(reply, replylen, "\"epc\":%.2f,", gpsdata->fix.epc); + + if (isnan(gpsdata->cust_param.velN) == 0) + str_appendf(reply, replylen, "\"velN\":%.2f,", gpsdata->cust_param.velN); + + if (isnan(gpsdata->cust_param.velE) == 0) + str_appendf(reply, replylen, "\"velE\":%.2f,", gpsdata->cust_param.velE); + + if (isnan(gpsdata->cust_param.height) == 0) + str_appendf(reply, replylen, "\"height\":%.2f,", gpsdata->cust_param.height); + #ifdef TIMING_ENABLE if (policy->timing) { char rtime_str[TIMESPEC_LEN]; diff -Naur A/gps.h C/gps.h --- A/gps.h 2019-06-08 17:48:57.651643221 +0530 +++ C/gps.h 2019-07-03 19:46:15.894362613 +0530 @@ -64,7 +64,6 @@ * timestamp_t and PPS do not play well together */ typedef double timestamp_t; /* Unix time in seconds with fractional part */ - struct gps_fix_t { timestamp_t time; /* Time of update */ int mode; /* Mode of fix */ @@ -1898,6 +1897,12 @@ char remote[GPS_PATH_MAX]; /* ...if this was passthrough */ }; +struct cust_param_t { + double velN; /*North component of velocity vector*/ + double velE; /*East velocity component*/ + double height; /*Height above ellipsoid*/ +}; + #ifndef TIMEDELTA_DEFINED #define TIMEDELTA_DEFINED @@ -1934,7 +1939,6 @@ /* * Main structure that includes all previous substructures */ - struct gps_data_t { gps_mask_t set; /* has field been set since this was last cleared? */ #define ONLINE_SET (1llu<<1) @@ -1987,7 +1991,7 @@ #else void* gps_fd; #endif - struct gps_fix_t fix; /* accumulated PVT data */ + struct gps_fix_t fix; /* accumulated PVT data */ /* this should move to the per-driver structure */ double separation; /* Geoidal separation, MSL - WGS84 (Meters) */ @@ -2037,10 +2041,11 @@ struct timedelta_t toff; struct timedelta_t pps; }; - /* FIXME! next lib rev need to add a place to put PPS precision */ + /* FIXME! next lib rev need to add a place to put PPS precision */ /* Private data - client code must not set this */ void *privdata; + struct cust_param_t cust_param; }; extern int gps_open(const char *, const char *, diff -Naur A/gpsutils.c C/gpsutils.c --- A/gpsutils.c 2019-06-08 17:48:57.655643221 +0530 +++ C/gpsutils.c 2019-07-03 14:13:56.773531633 +0530 @@ -290,7 +290,7 @@ if ((transfer & VERR_SET) != 0) to->epv = from->epv; if ((transfer & SPEEDERR_SET) != 0) - to->eps = from->eps; + to->eps = from->eps; } /* NOTE: timestamp_t is a double, so this is only precise to diff -Naur A/libgpsd_core.c C/libgpsd_core.c --- A/libgpsd_core.c 2019-06-08 17:48:57.659643221 +0530 +++ C/libgpsd_core.c 2019-07-03 19:21:55.293033355 +0530 @@ -1355,6 +1355,13 @@ && session->device_type->parse_packet != NULL) received |= session->device_type->parse_packet(session); + gpsd_log(&session->context->errout, LOG_INF, + "gpsd_poll :height:%lf, %velE:%lf velN:%lf\n", + session->gpsdata.cust_param.height, + session->gpsdata.cust_param.velE, + session->gpsdata.cust_param.velN); + + #ifdef RECONFIGURE_ENABLE /* * We may want to revert to the last driver that was marked @@ -1409,6 +1416,13 @@ // "transfer mask: %02x\n", session->gpsdata.set); gps_merge_fix(&session->gpsdata.fix, session->gpsdata.set, &session->newdata); + + gpsd_log(&session->context->errout, LOG_INF, + "After Merge fix*******:height:%lf, %velE:%lf velN:%lf session->gpsdata.fix.mode =%x session->newdata.mode = %x\r\n", + session->gpsdata.cust_param.height, + session->gpsdata.cust_param.velE , + session->gpsdata.cust_param.velN, session->gpsdata.fix.mode, session->newdata.mode); + #ifndef NOFLOATS_ENABLE gpsd_error_model(session, &session->gpsdata.fix, &session->oldfix); #endif /* NOFLOATS_ENABLE */ @@ -1481,6 +1495,7 @@ device->ntrip.conn_state = ntrip_conn_init; return DEVICE_ERROR; } else { + gpsd_log(&device->context->errout, LOG_INF,"Device ready..\n"); return DEVICE_READY; } } @@ -1490,12 +1505,12 @@ gps_mask_t changed = gpsd_poll(device); if (changed == EOF_IS) { - gpsd_log(&device->context->errout, LOG_WARN, + gpsd_log(&device->context->errout, LOG_INF, "device signed off %s\n", device->gpsdata.dev.path); return DEVICE_EOF; } else if (changed == ERROR_SET) { - gpsd_log(&device->context->errout, LOG_WARN, + gpsd_log(&device->context->errout, LOG_INF, "device read of %s returned error or packet sniffer failed sync (flags %s)\n", device->gpsdata.dev.path, gps_maskdump(changed)); @@ -1506,7 +1521,7 @@ * fd may have been in an end-of-file condition on select. */ if (fragments == 0) { - gpsd_log(&device->context->errout, LOG_DATA, + gpsd_log(&device->context->errout, LOG_INF, "%s returned zero bytes\n", device->gpsdata.dev.path); if (device->zerokill) { @@ -1515,11 +1530,11 @@ if (device->ntrip.works) { device->ntrip.works = false; // reset so we try this once only if (gpsd_activate(device, O_CONTINUE) < 0) { - gpsd_log(&device->context->errout, LOG_WARN, + gpsd_log(&device->context->errout, LOG_INF, "reconnect to ntrip server failed\n"); return DEVICE_ERROR; } else { - gpsd_log(&device->context->errout, LOG_INFO, + gpsd_log(&device->context->errout, LOG_INF, "reconnecting to ntrip server\n"); return DEVICE_READY; } @@ -1553,7 +1568,7 @@ /* must have a full packet to continue */ if ((changed & PACKET_SET) == 0) - break; + break; /* conditional prevents mask dumper from eating CPU */ if (device->context->errout.debug >= LOG_DATA) { diff -Naur A/libgps_json.c C/libgps_json.c --- A/libgps_json.c 2019-07-03 14:30:42.019103023 +0530 +++ C/libgps_json.c 2019-07-03 19:38:01.247608109 +0530 @@ -59,11 +59,17 @@ {"epd", t_real, .addr.real = &gpsdata->fix.epd, .dflt.real = NAN}, {"eps", t_real, .addr.real = &gpsdata->fix.eps, - .dflt.real = NAN}, - {"epc", t_real, .addr.real = &gpsdata->fix.epc, - .dflt.real = NAN}, + .dflt.real = NAN}, + {"epc", t_real, .addr.real = &gpsdata->fix.epc, + .dflt.real = NAN}, {"mode", t_integer, .addr.integer = &gpsdata->fix.mode, .dflt.integer = MODE_NOT_SEEN}, + {"velN", t_real, .addr.real = &gpsdata->cust_param.velN, + .dflt.real = NAN}, + {"velE", t_real, .addr.real = &gpsdata->cust_param.velE, + .dflt.real = NAN}, + {"height", t_real, .addr.real = &gpsdata->cust_param.height, + .dflt.real = NAN}, {NULL}, /* *INDENT-ON* */ }; @@ -434,6 +440,7 @@ gpsdata->set |= CLIMBERR_SET; if (isnan(gpsdata->fix.epc) == 0) gpsdata->set |= CLIMBERR_SET; + if (gpsdata->fix.mode != MODE_NOT_SEEN) gpsdata->set |= MODE_SET; return status; diff -Naur A/libgps_sock.c C/libgps_sock.c --- A/libgps_sock.c 2019-06-08 17:48:57.659643221 +0530 +++ C/libgps_sock.c 2019-07-03 14:27:21.125894375 +0530 @@ -33,6 +33,8 @@ #ifdef SOCKET_EXPORT_ENABLE #include "gps_json.h" +#define LIBGPS_DEBUG + struct privdata_t { bool newstyle; @@ -134,10 +136,11 @@ int gps_sock_read(struct gps_data_t *gpsdata) /* wait for and read data being streamed from the daemon */ { + + libgps_debug_trace((DEBUG_CALLS, "****gps_sock_read\n")); char *eol; ssize_t response_length; int status = -1; - gpsdata->set &= ~PACKET_SET; for (eol = PRIVATE(gpsdata)->buffer; *eol != '\n' && eol < PRIVATE(gpsdata)->buffer + PRIVATE(gpsdata)->waiting; eol++) @@ -160,7 +163,6 @@ sizeof(PRIVATE(gpsdata)->buffer) - PRIVATE(gpsdata)->waiting); #endif - /* if we just received data from the socket, it's in the buffer */ if (status > -1) PRIVATE(gpsdata)->waiting += status; @@ -192,6 +194,7 @@ if (eol == NULL) return 0; } + assert(eol != NULL); *eol = '\0';