* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/
-#define _GNU_SOURCE
-
#include <config.h>
#include <unistd.h>
#include <stdlib.h>
#include <math.h>
#include <errno.h>
#include <glib/gstdio.h>
-#include <gtk/gtk.h>
#include <fcntl.h>
-#include <libxml/parser.h>
#include "utils.h"
-#include "bt.h"
#include "gps.h"
-#include "mapper-types.h"
-#include "ui-common.h"
-#include "map.h"
-#include "track.h"
-#include "route.h"
-#include "gps-panels.h"
-#include "settings.h"
-#include "gps-conn.h"
-#include "gtkgps.h"
-#include "gtkcompass.h"
-
-#define GPS_FILTER_MAX_SKIP (10)
-static gint track_drop_cnt=0;
+#include "gps-nmea-parse.h"
-gboolean
-channel_cb_error(GIOChannel *src, GIOCondition condition, gpointer data)
-{
-/* An error has occurred - re-connect(). */
-rcvr_disconnect();
-track_add(0, FALSE);
-_speed_excess = FALSE;
-
-if (_conn_state > RCVR_OFF) {
- gps_conn_set_state(RCVR_DOWN);
- gps_hide_text();
- rcvr_connect_later();
-}
-return FALSE;
-}
+#define DELIM ","
+
+#define NMEA_RMC "$GPRMC"
+#define NMEA_GGA "$GPGGA"
+#define NMEA_GSA "$GPGSA"
+#define NMEA_GSV "$GPGSV"
+#define NMEA_PRLEN (6)
-static void
-channel_parse_rmc(gchar * sentence)
+static void
+gps_nmea_parse_rmc(Gps *gps, gchar *sentence)
{
/* Recommended Minimum Navigation Information C
* 1) UTC Time
gdouble tmpd = 0.f;
guint tmpi = 0;
gboolean newly_fixed = FALSE;
- vprintf("%s(): %s\n", __PRETTY_FUNCTION__, sentence);
-
-#define DELIM ","
+ g_debug("%s(): %s", __PRETTY_FUNCTION__, sentence);
/* Parse time. */
token = strsep(&sentence, DELIM);
/* Token is now Status. */
if (token && *token == 'A') {
/* Data is valid. */
- if (_conn_state != RCVR_FIXED) {
- newly_fixed = TRUE;
- gps_conn_set_state(RCVR_FIXED);
+ if (gps->io.conn != RCVR_FIXED) {
+ gps->data.newly_fixed = TRUE;
+ gps_conn_set_state(gps, RCVR_FIXED);
+ } else {
+ gps->data.newly_fixed = FALSE;
}
} else {
/* Data is invalid - not enough satellites?. */
- if (_conn_state != RCVR_UP) {
- gps_conn_set_state(RCVR_UP);
- track_add(0, FALSE);
+ if (gps->io.conn != RCVR_UP) {
+ gps_conn_set_state(gps, RCVR_UP);
+ /* track_add(NULL, FALSE); */
}
}
MACRO_PARSE_FLOAT(tmpd, dpoint - 2);
dpoint[-2] = '\0';
MACRO_PARSE_INT(tmpi, token);
- _gps.lat = tmpi + (tmpd * (1.0 / 60.0));
+ gps->data.lat = tmpi + (tmpd * (1.0 / 60.0));
}
/* Parse N or S. */
token = strsep(&sentence, DELIM);
if (token && *token == 'S')
- _gps.lat = -_gps.lat;
+ gps->data.lat = -gps->data.lat;
/* Parse the longitude. */
token = strsep(&sentence, DELIM);
MACRO_PARSE_FLOAT(tmpd, dpoint - 2);
dpoint[-2] = '\0';
MACRO_PARSE_INT(tmpi, token);
- _gps.lon = tmpi + (tmpd * (1.0 / 60.0));
+ gps->data.lon = tmpi + (tmpd * (1.0 / 60.0));
}
/* Parse E or W. */
token = strsep(&sentence, DELIM);
if (token && *token == 'W')
- _gps.lon = -_gps.lon;
+ gps->data.lon = -gps->data.lon;
/* Parse speed over ground, knots. */
token = strsep(&sentence, DELIM);
if (token && *token) {
- MACRO_PARSE_FLOAT(_gps.speed, token);
- if (_gps.fix > 1) {
- _gps.maxspeed = MAX(_gps.maxspeed, _gps.speed);
- gps_display_data_speed(info_banner.speed, _gps.speed);
+ MACRO_PARSE_FLOAT(gps->data.speed, token);
+ if (gps->data.fix > FIX_NOFIX) {
+ gps->data.maxspeed=MAX(gps->data.maxspeed, gps->data.speed);
}
}
/* Parse heading, degrees from true north. */
token = strsep(&sentence, DELIM);
if (token && *token) {
- MACRO_PARSE_FLOAT(_gps.heading, token);
+ MACRO_PARSE_FLOAT(gps->data.heading, token);
}
/* Parse date. */
gpsdate[6] = '\0'; /* Make sure time is 6 chars long. */
strcat(gpsdate, token);
strptime(gpsdate, "%H%M%S%d%m%y", &time);
- _pos.time = mktime(&time) + _gmtoffset;
- } else
- _pos.time = time(NULL);
-
- /* Add new data to track only if we have a fix */
- _track_store=TRUE;
-
- /* XXX: Set filter logic somewhere else */
-
- if ((_conn_state == RCVR_FIXED) && (_track_store==TRUE)) {
- if ((_gps_filter==TRUE) && (track_drop_cnt<GPS_FILTER_MAX_SKIP)) {
- integerize_data(&_gps, &_pos);
- if ( (_gps.hdop<_filter_hdop || _filter_hdop==0.0) &&
- (_gps.vdop<_filter_vdop || _filter_vdop==0.0) &&
- (fabs(_gps.heading-_gps.lheading)>_filter_angle || _filter_angle==0.0 ) &&
- (_map_location_known==TRUE && (_map_location_dist<_filter_osm || _map_location_dist==0.0)) ) {
- track_add(_pos.time, newly_fixed);
- _gps.lheading=_gps.heading;
- track_drop_cnt=0;
- } else {
- track_drop_cnt++;
- g_printf("*** Filtering by: [%s %s %s %s] A: %f (%d)\n",
- _gps.hdop>_filter_hdop ? "HDOP" : "-",
- _gps.vdop>_filter_vdop ? "VDOP" : "-",
- (fabs(_gps.heading-_gps.lheading)<_filter_angle) ? "Angle" : "-",
- (_map_location_known==TRUE && (_map_location_dist>_filter_osm)) ? "OSM" : "-",
- fabs(_gps.heading-_gps.lheading), track_drop_cnt);
- }
- map_refresh_mark();
- } else {
- track_drop_cnt=0;
- integerize_data(&_gps, &_pos);
- track_add(_pos.time, newly_fixed);
- _gps.lheading=_gps.heading;
- map_refresh_mark();
- }
+ gps->data.time = mktime(&time) + _gmtoffset;
+ } else {
+ gps->data.time = time(NULL);
}
- vprintf("%s(): return\n", __PRETTY_FUNCTION__);
+ gps_data_integerize(&gps->data);
+ gps->data.lheading=gps->data.heading;
}
-static void
-channel_parse_gga(gchar * sentence)
+static void
+gps_nmea_parse_gga(Gps *gps, gchar * sentence)
{
/* GGA Global Positioning System Fix Data
1. Fix Time
15. the checksum data
*/
gchar *token;
- vprintf("%s(): %s\n", __PRETTY_FUNCTION__, sentence);
-
-#define DELIM ","
+ g_debug("%s(): %s", __PRETTY_FUNCTION__, sentence);
/* Skip Fix time */
token = strsep(&sentence, DELIM);
/* Parse Fix quality */
token = strsep(&sentence, DELIM);
if (token && *token)
- MACRO_PARSE_INT(_gps.fixquality, token);
+ MACRO_PARSE_INT(gps->data.fixquality, token);
/* Skip number of satellites */
token = strsep(&sentence, DELIM);
/* Parse Horizontal dilution of position */
token = strsep(&sentence, DELIM);
if (token && *token)
- MACRO_PARSE_INT(_gps.hdop, token);
+ MACRO_PARSE_INT(gps->data.hdop, token);
/* Altitude */
token = strsep(&sentence, DELIM);
if (token && *token) {
- MACRO_PARSE_FLOAT(_pos.altitude, token);
+ MACRO_PARSE_FLOAT(gps->data.altitude, token);
} else
- _pos.altitude = NAN;
-
- vprintf("%s(): return\n", __PRETTY_FUNCTION__);
+ gps->data.altitude = NAN;
}
-static void
-channel_parse_gsa(gchar * sentence)
+static void
+gps_nmea_parse_gsa(Gps *gps, gchar * sentence)
{
/* GPS DOP and active satellites
* 1) Auto selection of 2D or 3D fix (M = manual)
guint i,si;
gint satforfix[12];
- vprintf("%s(): %s\n", __PRETTY_FUNCTION__, sentence);
-
-#define DELIM ","
+ g_debug("%s(): %s\n", __PRETTY_FUNCTION__, sentence);
/* Skip Auto selection. */
token = strsep(&sentence, DELIM);
/* 3D fix. */
token = strsep(&sentence, DELIM);
if (token && *token)
- MACRO_PARSE_INT(_gps.fix, token);
+ MACRO_PARSE_INT(gps->data.fix, token);
- _gps.satinuse = 0;
+ gps->data.satinuse = 0;
for (i = 0; i < 12; i++) {
gint fprn;
token = strsep(&sentence, DELIM);
else
fprn=-1;
satforfix[i]=fprn;
- _gps.sat[i].fix=FALSE;
+ gps->data.sat[i].fix=FALSE;
}
for (i=0;i<12;i++)
for (si=0;si<12;si++) {
- if (_gps.sat[i].prn==satforfix[si])
- _gps.sat[i].fix=TRUE;
+ if (gps->data.sat[i].prn==satforfix[si])
+ gps->data.sat[i].fix=TRUE;
}
/* PDOP */
token = strsep(&sentence, DELIM);
if (token && *token)
- MACRO_PARSE_FLOAT(_gps.pdop, token);
+ MACRO_PARSE_FLOAT(gps->data.pdop, token);
/* HDOP */
token = strsep(&sentence, DELIM);
if (token && *token)
- MACRO_PARSE_FLOAT(_gps.hdop, token);
+ MACRO_PARSE_FLOAT(gps->data.hdop, token);
/* VDOP */
token = strsep(&sentence, DELIM);
if (token && *token)
- MACRO_PARSE_FLOAT(_gps.vdop, token);
-
- vprintf("%s(): return\n", __PRETTY_FUNCTION__);
+ MACRO_PARSE_FLOAT(gps->data.vdop, token);
}
-static void
-channel_parse_gsv(gchar * sentence)
+static void
+gps_nmea_parse_gsv(Gps *gps, gchar * sentence)
{
/* Must be GSV - Satellites in view
* 1) total number of messages
static guint running_total = 0;
static guint num_sats_used = 0;
static guint satcnt = 0;
- g_printf("%s(): %s\n", __PRETTY_FUNCTION__, sentence);
+ /* g_printf("%s(): %s\n", __PRETTY_FUNCTION__, sentence); */
/* Parse number of messages. */
token = strsep(&sentence, DELIM);
/* Parse number of satellites in view. */
token = strsep(&sentence, DELIM);
if (token && *token) {
- MACRO_PARSE_INT(_gps.satinview, token);
- if (_gps.satinview > 12) /* Handle buggy NMEA. */
- _gps.satinview = 12;
+ MACRO_PARSE_INT(gps->data.satinview, token);
+ if (gps->data.satinview > 12) /* Handle buggy NMEA. */
+ gps->data.satinview = 12;
}
/* Loop until there are no more satellites to parse. */
/* Get token for Satellite Number. */
token = strsep(&sentence, DELIM);
if (token && *token)
- _gps.sat[satcnt].prn = atoi(token);
+ gps->data.sat[satcnt].prn = atoi(token);
/* Get token for elevation in degrees (0-90). */
token = strsep(&sentence, DELIM);
if (token && *token)
- _gps.sat[satcnt].elevation = atoi(token);
+ gps->data.sat[satcnt].elevation = atoi(token);
/* Get token for azimuth in degrees to true north (0-359). */
token = strsep(&sentence, DELIM);
if (token && *token)
- _gps.sat[satcnt].azimuth = atoi(token);
+ gps->data.sat[satcnt].azimuth = atoi(token);
/* Get token for SNR. */
token = strsep(&sentence, DELIM);
- if (token && *token && (_gps.sat[satcnt].snr = atoi(token))) {
+ if (token && *token && (gps->data.sat[satcnt].snr = atoi(token))) {
/* SNR is non-zero - add to total and count as used. */
- running_total += _gps.sat[satcnt].snr;
+ running_total += gps->data.sat[satcnt].snr;
num_sats_used++;
}
satcnt++;
if (msgcnt == nummsgs) {
/* This is the last message. Calculate signal strength. */
if (num_sats_used) {
- if (_conn_state == RCVR_UP) {
+ if (gps->io.conn==RCVR_UP) {
gdouble fraction = running_total * sqrt(num_sats_used) / num_sats_used / 100.0;
BOUND(fraction, 0.0, 1.0);
- gps_conn_set_progress(fraction);
+ if (gps->connection_progress!=NULL)
+ gps->connection_progress(gps, fraction);
}
- running_total = 0;
- num_sats_used = 0;
+ running_total=0;
+ num_sats_used=0;
}
satcnt = 0;
-
- /* Keep awake while they watch the progress bar. */
- KEEP_DISPLAY_ON();
}
-
- g_printf("%s(): return\n", __PRETTY_FUNCTION__);
}
gboolean
-channel_cb_input(GIOChannel *src, GIOCondition condition, gpointer data)
+gps_nmea_parse(Gps *gps)
{
-gsize bytes_read;
-GIOStatus status;
-gchar *eol;
-
-vprintf("%s(%d)\n", __PRETTY_FUNCTION__, condition);
-
-status=g_io_channel_read_chars(_channel, _gps_read_buf_curr, _gps_read_buf_last - _gps_read_buf_curr, &bytes_read, NULL);
-
-switch (status) {
- case G_IO_STATUS_NORMAL:
- _gps_read_buf_curr += bytes_read;
- *_gps_read_buf_curr = '\0'; /* append a \0 so we can read as string */
- while ((eol = strchr(_gps_read_buf, '\n'))) {
- gchar *sptr = _gps_read_buf + 1; /* Skip the $ */
- guint csum = 0;
- if (*_gps_read_buf == '$') {
- /* This is the beginning of a sentence; okay to parse. */
- *eol = '\0'; /* overwrite \n with \0 */
- while (*sptr && *sptr != '*')
- csum ^= *sptr++;
-
- /* If we're at a \0 (meaning there is no checksum), or if the
- * checksum is good, then parse the sentence. */
- if (!*sptr || csum == strtol(sptr + 1, NULL, 16)) {
- if (*sptr)
- *sptr = '\0'; /* take checksum out of the buffer. */
- if (!strncmp(_gps_read_buf + 3, "GSV", 3)) {
- if (_conn_state == RCVR_UP)
- channel_parse_gsv(_gps_read_buf + 7);
- } else if (!strncmp(_gps_read_buf + 3, "RMC", 3))
- channel_parse_rmc(_gps_read_buf + 7);
- else if (!strncmp(_gps_read_buf + 3, "GGA", 3))
- channel_parse_gga(_gps_read_buf + 7);
- else if (!strncmp(_gps_read_buf + 3, "GSA", 3))
- channel_parse_gsa(_gps_read_buf + 7);
- else g_print("Unknown NMEA: [%s]\n", _gps_read_buf);
-
- if (_gps_info)
- gps_display_data();
-
- if (_satdetails_on)
- gps_display_details();
- } else {
- /* There was a checksum, and it was bad. */
- g_printerr("%s: Bad checksum in NMEA sentence:\n%s\n", __PRETTY_FUNCTION__, _gps_read_buf);
- }
- }
+g_assert(gps);
- /* If eol is at or after (_gps_read_buf_curr - 1) */
- if (eol >= (_gps_read_buf_curr - 1)) {
- /* Last read was a newline - reset read buffer */
- _gps_read_buf_curr = _gps_read_buf;
- *_gps_read_buf_curr = '\0';
- } else {
- /* Move the next line to the front of the buffer. */
- memmove(_gps_read_buf, eol + 1, _gps_read_buf_curr - eol); /* include terminating 0 */
- /* Subtract _curr so that it's pointing at the new \0. */
- _gps_read_buf_curr -= (eol - _gps_read_buf + 1);
- }
- }
- break;
- case G_IO_STATUS_ERROR:
- case G_IO_STATUS_EOF:
- rcvr_disconnect();
- rcvr_connect_later();
- return FALSE;
- break;
- case G_IO_STATUS_AGAIN:
- return TRUE;
- break;
-}
+g_debug("NMEA2 %d %s", gps->io.nmea_cnt, gps->io.nmea);
+
+if (!gps->io.nmea)
+ return FALSE;
+
+if (strlen(gps->io.nmea)<NMEA_PRLEN+1)
+ return FALSE;
+
+if (!strncmp(gps->io.nmea, NMEA_GSV, NMEA_PRLEN)) {
+ gps_nmea_parse_gsv(gps, gps->io.nmea + NMEA_PRLEN+1);
+ if (gps->update_satellite)
+ g_idle_add_full(G_PRIORITY_DEFAULT, (GSourceFunc)gps->update_satellite, gps, NULL);
+
+} else if (!strncmp(gps->io.nmea, NMEA_RMC, NMEA_PRLEN)) {
+ gps_nmea_parse_rmc(gps, gps->io.nmea + NMEA_PRLEN+1);
+ if ((gps->io.conn==RCVR_FIXED) && (gps->update_location!=NULL))
+ gps->update_location(gps);
-vprintf("%s(): return TRUE\n", __PRETTY_FUNCTION__);
-return TRUE;
+} else if (!strncmp(gps->io.nmea, NMEA_GGA, NMEA_PRLEN)) {
+ gps_nmea_parse_gga(gps, gps->io.nmea + NMEA_PRLEN+1);
+ if (gps->update_info)
+ g_idle_add_full(G_PRIORITY_DEFAULT, (GSourceFunc)gps->update_info, gps, NULL);
+
+} else if (!strncmp(gps->io.nmea, NMEA_GSA, NMEA_PRLEN)) {
+ gps_nmea_parse_gsa(gps, gps->io.nmea + NMEA_PRLEN+1);
+ if (gps->update_satellite)
+ g_idle_add_full(G_PRIORITY_DEFAULT, (GSourceFunc)gps->update_satellite, gps, NULL);
+ if (gps->update_info)
+ g_idle_add_full(G_PRIORITY_DEFAULT, (GSourceFunc)gps->update_info, gps, NULL);
+
+} else g_printerr("Unknown NMEA: [%s]\n", gps->io.nmea);
+
+return FALSE;
}
projects / mapper / blobdiff