mirror of https://github.com/wb2osz/direwolf.git
819 lines
20 KiB
C
819 lines
20 KiB
C
//
|
||
// This file is part of Dire Wolf, an amateur radio packet TNC.
|
||
//
|
||
// Copyright (C) 2013, 2014, 2015 John Langner, WB2OSZ
|
||
//
|
||
// This program is free software: you can redistribute it and/or modify
|
||
// it under the terms of the GNU General Public License as published by
|
||
// the Free Software Foundation, either version 2 of the License, or
|
||
// (at your option) any later version.
|
||
//
|
||
// This program is distributed in the hope that it will be useful,
|
||
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||
// GNU General Public License for more details.
|
||
//
|
||
// You should have received a copy of the GNU General Public License
|
||
// along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||
//
|
||
|
||
|
||
/*------------------------------------------------------------------
|
||
*
|
||
* Module: dwgpsnmea.c
|
||
*
|
||
* Purpose: process NMEA sentences from a GPS receiver.
|
||
*
|
||
* Description: This version is available for all operating systems.
|
||
*
|
||
*---------------------------------------------------------------*/
|
||
|
||
|
||
#include "direwolf.h"
|
||
|
||
|
||
#include <stdio.h>
|
||
#include <unistd.h>
|
||
#include <stdlib.h>
|
||
#include <assert.h>
|
||
#include <string.h>
|
||
#include <errno.h>
|
||
#include <time.h>
|
||
|
||
|
||
#include "textcolor.h"
|
||
#include "dwgps.h"
|
||
#include "dwgpsnmea.h"
|
||
#include "serial_port.h"
|
||
|
||
|
||
static int s_debug = 0; /* Enable debug output. */
|
||
/* See dwgpsnmea_init description for values. */
|
||
|
||
static struct misc_config_s *s_save_configp;
|
||
|
||
|
||
|
||
#if __WIN32__
|
||
static unsigned __stdcall read_gpsnmea_thread (void *arg);
|
||
#else
|
||
static void * read_gpsnmea_thread (void *arg);
|
||
#endif
|
||
|
||
/*-------------------------------------------------------------------
|
||
*
|
||
* Name: dwgpsnmea_init
|
||
*
|
||
* Purpose: Open serial port for the GPS receiver.
|
||
*
|
||
* Inputs: pconfig Configuration settings. This includes
|
||
* serial port name for direct connect.
|
||
*
|
||
* debug - If >= 1, print results when dwgps_read is called.
|
||
* (In different file.)
|
||
*
|
||
* If >= 2, location updates are also printed.
|
||
* (In this file.)
|
||
* Why not do it in dwgps_set_data() ?
|
||
* Here, we can prefix it with GPSNMEA to
|
||
* distinguish it from GPSD.
|
||
*
|
||
* If >= 3, Also the NMEA sentences.
|
||
* (In this file.)
|
||
*
|
||
* Returns: 1 = success
|
||
* 0 = nothing to do (no serial port specified in config)
|
||
* -1 = failure
|
||
*
|
||
* Description: When talking directly to GPS receiver (any operating system):
|
||
*
|
||
* - Open the appropriate serial port.
|
||
* - Start up thread to process incoming data.
|
||
* It reads from the serial port and deposits into
|
||
* dwgps_info, above.
|
||
*
|
||
* The application calls dwgps_read to get the most recent information.
|
||
*
|
||
*--------------------------------------------------------------------*/
|
||
|
||
/* Make this static and available to all functions so term function can access it. */
|
||
|
||
static MYFDTYPE s_gpsnmea_port_fd = MYFDERROR; /* Handle for serial port. */
|
||
|
||
|
||
int dwgpsnmea_init (struct misc_config_s *pconfig, int debug)
|
||
{
|
||
//dwgps_info_t info;
|
||
#if __WIN32__
|
||
HANDLE read_gps_th;
|
||
#else
|
||
pthread_t read_gps_tid;
|
||
//int e;
|
||
#endif
|
||
|
||
s_debug = debug;
|
||
s_save_configp = pconfig;
|
||
|
||
|
||
if (s_debug >= 2) {
|
||
text_color_set(DW_COLOR_DEBUG);
|
||
dw_printf ("dwgpsnmea_init()\n");
|
||
}
|
||
|
||
if (strlen(pconfig->gpsnmea_port) == 0) {
|
||
|
||
/* Nothing to do. Leave initial fix value for not init. */
|
||
return (0);
|
||
}
|
||
|
||
/*
|
||
* Open serial port connection.
|
||
* 4800 baud is standard for GPS.
|
||
* Should add an option to allow changing someday.
|
||
*/
|
||
|
||
s_gpsnmea_port_fd = serial_port_open (pconfig->gpsnmea_port, 4800);
|
||
|
||
if (s_gpsnmea_port_fd != MYFDERROR) {
|
||
#if __WIN32__
|
||
read_gps_th = (HANDLE)_beginthreadex (NULL, 0, read_gpsnmea_thread, (void*)(long)s_gpsnmea_port_fd, 0, NULL);
|
||
if (read_gps_th == NULL) {
|
||
text_color_set(DW_COLOR_ERROR);
|
||
dw_printf ("Could not create GPS NMEA listening thread.\n");
|
||
return (-1);
|
||
}
|
||
#else
|
||
int e;
|
||
e = pthread_create (&read_gps_tid, NULL, read_gpsnmea_thread, (void*)(long)s_gpsnmea_port_fd);
|
||
if (e != 0) {
|
||
text_color_set(DW_COLOR_ERROR);
|
||
perror("Could not create GPS NMEA listening thread.");
|
||
return (-1);
|
||
}
|
||
#endif
|
||
}
|
||
else {
|
||
text_color_set(DW_COLOR_ERROR);
|
||
dw_printf ("Could not open serial port %s for GPS receiver.\n", pconfig->gpsnmea_port);
|
||
return (-1);
|
||
}
|
||
|
||
/* success */
|
||
|
||
return (1);
|
||
|
||
} /* end dwgpsnmea_init */
|
||
|
||
|
||
/* Return fd to share if waypoint wants same device. */
|
||
/* Currently both are fixed speed at 4800. */
|
||
/* If that ever becomes configurable, that needs to be compared too. */
|
||
|
||
MYFDTYPE dwgpsnmea_get_fd(char *wp_port_name, int speed)
|
||
{
|
||
if (strcmp(s_save_configp->gpsnmea_port, wp_port_name) == 0 && speed == 4800) {
|
||
return (s_gpsnmea_port_fd);
|
||
}
|
||
return (MYFDERROR);
|
||
}
|
||
|
||
|
||
/*-------------------------------------------------------------------
|
||
*
|
||
* Name: read_gpsnmea_thread
|
||
*
|
||
* Purpose: Read information from GPS, as it becomes available, and
|
||
* store it for later retrieval by dwgps_read.
|
||
*
|
||
* Inputs: fd - File descriptor for serial port.
|
||
*
|
||
* Description: This version reads from serial port and parses the
|
||
* NMEA sentences.
|
||
*
|
||
*--------------------------------------------------------------------*/
|
||
|
||
#define TIMEOUT 5
|
||
|
||
|
||
#if __WIN32__
|
||
static unsigned __stdcall read_gpsnmea_thread (void *arg)
|
||
#else
|
||
static void * read_gpsnmea_thread (void *arg)
|
||
#endif
|
||
{
|
||
MYFDTYPE fd = (MYFDTYPE)(long)arg;
|
||
|
||
// Maximum length of message from GPS receiver is 82 according to some people.
|
||
// Make buffer considerably larger to be safe.
|
||
|
||
#define NMEA_MAX_LEN 160
|
||
|
||
char gps_msg[NMEA_MAX_LEN];
|
||
int gps_msg_len = 0;
|
||
dwgps_info_t info;
|
||
|
||
|
||
if (s_debug >= 2) {
|
||
text_color_set(DW_COLOR_DEBUG);
|
||
dw_printf ("read_gpsnmea_thread (%d)\n", (int)(long)arg);
|
||
}
|
||
|
||
dwgps_clear (&info);
|
||
info.fix = DWFIX_NOT_SEEN; /* clear not init state. */
|
||
if (s_debug >= 2) {
|
||
text_color_set(DW_COLOR_DEBUG);
|
||
dwgps_print ("GPSNMEA: ", &info);
|
||
}
|
||
dwgps_set_data (&info);
|
||
|
||
|
||
while (1) {
|
||
int ch;
|
||
|
||
ch = serial_port_get1(fd);
|
||
|
||
if (ch < 0) {
|
||
|
||
/* This might happen if a USB device is unplugged. */
|
||
/* I can't imagine anything that would cause it with */
|
||
/* a normal serial port. */
|
||
|
||
text_color_set(DW_COLOR_ERROR);
|
||
dw_printf ("----------------------------------------------\n");
|
||
dw_printf ("GPSNMEA: Lost communication with GPS receiver.\n");
|
||
dw_printf ("----------------------------------------------\n");
|
||
|
||
info.fix = DWFIX_ERROR;
|
||
if (s_debug >= 2) {
|
||
text_color_set(DW_COLOR_DEBUG);
|
||
dwgps_print ("GPSNMEA: ", &info);
|
||
}
|
||
dwgps_set_data (&info);
|
||
|
||
// TODO: doesn't exist yet - serial_port_close(fd);
|
||
s_gpsnmea_port_fd = MYFDERROR;
|
||
|
||
break; /* terminate thread. */
|
||
}
|
||
|
||
if (ch == '$') {
|
||
// Start of new sentence.
|
||
gps_msg_len = 0;
|
||
gps_msg[gps_msg_len++] = ch;
|
||
gps_msg[gps_msg_len] = '\0';
|
||
}
|
||
else if (ch == '\r' || ch == '\n') {
|
||
if (gps_msg_len >= 6 && gps_msg[0] == '$') {
|
||
|
||
dwfix_t f;
|
||
|
||
if (s_debug >= 3) {
|
||
text_color_set(DW_COLOR_DEBUG);
|
||
dw_printf ("%s\n", gps_msg);
|
||
}
|
||
|
||
/* Process sentence. */
|
||
|
||
if (strncmp(gps_msg, "$GPRMC", 6) == 0 || strncmp(gps_msg, "$GNRMC", 6) == 0) {
|
||
|
||
f = dwgpsnmea_gprmc (gps_msg, 0, &info.dlat, &info.dlon, &info.speed_knots, &info.track);
|
||
|
||
if (f == DWFIX_ERROR) {
|
||
|
||
/* Parse error. Shouldn't happen. Better luck next time. */
|
||
text_color_set(DW_COLOR_INFO);
|
||
dw_printf ("GPSNMEA: Error parsing $GPRMC sentence.\n");
|
||
dw_printf ("%s\n", gps_msg);
|
||
}
|
||
else if (f == DWFIX_2D) {
|
||
|
||
if (info.fix != DWFIX_2D && info.fix != DWFIX_3D) {
|
||
|
||
text_color_set(DW_COLOR_INFO);
|
||
dw_printf ("GPSNMEA: Location fix is now available.\n");
|
||
|
||
info.fix = DWFIX_2D; // Don't know if 2D or 3D. Take minimum.
|
||
}
|
||
info.timestamp = time(NULL);
|
||
if (s_debug >= 2) {
|
||
text_color_set(DW_COLOR_DEBUG);
|
||
dwgps_print ("GPSNMEA: ", &info);
|
||
}
|
||
dwgps_set_data (&info);
|
||
}
|
||
else {
|
||
|
||
if (info.fix == DWFIX_2D || info.fix == DWFIX_3D) {
|
||
|
||
text_color_set(DW_COLOR_INFO);
|
||
dw_printf ("GPSNMEA: Lost location fix.\n");
|
||
}
|
||
info.fix = f; /* lost it. */
|
||
info.timestamp = time(NULL);
|
||
if (s_debug >= 2) {
|
||
text_color_set(DW_COLOR_DEBUG);
|
||
dwgps_print ("GPSNMEA: ", &info);
|
||
}
|
||
dwgps_set_data (&info);
|
||
}
|
||
|
||
}
|
||
else if (strncmp(gps_msg, "$GPGGA", 6) == 0 || strncmp(gps_msg, "$GNGGA", 6) == 0) {
|
||
int nsat;
|
||
|
||
f = dwgpsnmea_gpgga (gps_msg, 0, &info.dlat, &info.dlon, &info.altitude, &nsat);
|
||
|
||
/* Only switch between 2D & 3D. */
|
||
/* Let GPRMC handle other changes in fix state and data transfer. */
|
||
|
||
if (f == DWFIX_ERROR) {
|
||
|
||
/* Parse error. Shouldn't happen. Better luck next time. */
|
||
text_color_set(DW_COLOR_INFO);
|
||
dw_printf ("GPSNMEA: Error parsing $GPGGA sentence.\n");
|
||
dw_printf ("%s\n", gps_msg);
|
||
}
|
||
else if ((f == DWFIX_3D && info.fix == DWFIX_2D) ||
|
||
(f == DWFIX_2D && info.fix == DWFIX_3D)) {
|
||
text_color_set(DW_COLOR_INFO);
|
||
dw_printf ("GPSNMEA: Location fix is now %dD.\n", (int)f);
|
||
info.fix = f;
|
||
}
|
||
}
|
||
}
|
||
|
||
gps_msg_len = 0;
|
||
gps_msg[gps_msg_len] = '\0';
|
||
}
|
||
else {
|
||
if (gps_msg_len < NMEA_MAX_LEN-1) {
|
||
gps_msg[gps_msg_len++] = ch;
|
||
gps_msg[gps_msg_len] = '\0';
|
||
}
|
||
}
|
||
} /* while (1) */
|
||
|
||
#if __WIN32__
|
||
return (0);
|
||
#else
|
||
return (NULL);
|
||
#endif
|
||
|
||
} /* end read_gpsnmea_thread */
|
||
|
||
|
||
|
||
|
||
/*-------------------------------------------------------------------
|
||
*
|
||
* Name: remove_checksum
|
||
*
|
||
* Purpose: Validate checksum and remove before further processing.
|
||
*
|
||
* Inputs: sentence
|
||
* quiet suppress printing of error messages.
|
||
*
|
||
* Outputs: sentence modified in place.
|
||
*
|
||
* Returns: 0 = good checksum.
|
||
* -1 = error. missing or wrong.
|
||
*
|
||
*--------------------------------------------------------------------*/
|
||
|
||
|
||
static int remove_checksum (char *sent, int quiet)
|
||
{
|
||
char *p;
|
||
unsigned char cs;
|
||
|
||
|
||
// Do we have valid checksum?
|
||
|
||
cs = 0;
|
||
for (p = sent+1; *p != '*' && *p != '\0'; p++) {
|
||
cs ^= *p;
|
||
}
|
||
|
||
p = strchr (sent, '*');
|
||
if (p == NULL) {
|
||
if ( ! quiet) {
|
||
text_color_set (DW_COLOR_INFO);
|
||
dw_printf("Missing GPS checksum.\n");
|
||
}
|
||
return (-1);
|
||
}
|
||
if (cs != strtoul(p+1, NULL, 16)) {
|
||
if ( ! quiet) {
|
||
text_color_set (DW_COLOR_ERROR);
|
||
dw_printf("GPS checksum error. Expected %02x but found %s.\n", cs, p+1);
|
||
}
|
||
return (-1);
|
||
}
|
||
*p = '\0'; // Remove the checksum.
|
||
return (0);
|
||
}
|
||
|
||
|
||
/*-------------------------------------------------------------------
|
||
*
|
||
* Name: dwgpsnmea_gprmc
|
||
*
|
||
* Purpose: Parse $GPRMC sentence and extract interesing parts.
|
||
*
|
||
* Inputs: sentence NMEA sentence.
|
||
*
|
||
* quiet suppress printing of error messages.
|
||
*
|
||
* Outputs: odlat latitude
|
||
* odlon longitude
|
||
* oknots speed
|
||
* ocourse direction of travel.
|
||
*
|
||
* Left undefined if not valid.
|
||
*
|
||
* Returns: DWFIX_ERROR Parse error.
|
||
* DWFIX_NO_FIX GPS is there but Position unknown. Could be temporary.
|
||
* DWFIX_2D Valid position. We don't know if it is really 2D or 3D.
|
||
*
|
||
* Examples: $GPRMC,001431.00,V,,,,,,,121015,,,N*7C
|
||
* $GPRMC,212404.000,V,4237.1505,N,07120.8602,W,,,150614,,*0B
|
||
* $GPRMC,000029.020,V,,,,,,,080810,,,N*45
|
||
* $GPRMC,003413.710,A,4237.1240,N,07120.8333,W,5.07,291.42,160614,,,A*7F
|
||
*
|
||
*--------------------------------------------------------------------*/
|
||
|
||
dwfix_t dwgpsnmea_gprmc (char *sentence, int quiet, double *odlat, double *odlon, float *oknots, float *ocourse)
|
||
{
|
||
char stemp[NMEA_MAX_LEN]; /* Make copy because parsing is destructive. */
|
||
|
||
char *next;
|
||
|
||
char *ptype; /* Should be $GPRMC */
|
||
char *ptime; /* Time, hhmmss[.sss] */
|
||
char *pstatus; /* Status, A=Active (valid position), V=Void */
|
||
char *plat; /* Latitude */
|
||
char *pns; /* North/South */
|
||
char *plon; /* Longitude */
|
||
char *pew; /* East/West */
|
||
char *pknots; /* Speed over ground, knots. */
|
||
char *pcourse; /* True course, degrees. */
|
||
char *pdate; /* Date, ddmmyy */
|
||
/* Magnetic variation */
|
||
/* In version 3.00, mode is added: A D E N (see below) */
|
||
/* Checksum */
|
||
|
||
strlcpy (stemp, sentence, sizeof(stemp));
|
||
|
||
if (remove_checksum (stemp, quiet) < 0) {
|
||
return (DWFIX_ERROR);
|
||
}
|
||
|
||
next = stemp;
|
||
ptype = strsep(&next, ",");
|
||
ptime = strsep(&next, ",");
|
||
pstatus = strsep(&next, ",");
|
||
plat = strsep(&next, ",");
|
||
pns = strsep(&next, ",");
|
||
plon = strsep(&next, ",");
|
||
pew = strsep(&next, ",");
|
||
pknots = strsep(&next, ",");
|
||
pcourse = strsep(&next, ",");
|
||
pdate = strsep(&next, ",");
|
||
|
||
/* Suppress the 'set but not used' warnings. */
|
||
/* Alternatively, we might use __attribute__((unused)) */
|
||
|
||
(void)(ptype);
|
||
(void)(ptime);
|
||
(void)(pdate);
|
||
|
||
if (pstatus != NULL && strlen(pstatus) == 1) {
|
||
if (*pstatus != 'A') {
|
||
return (DWFIX_NO_FIX); /* Not "Active." Don't parse. */
|
||
}
|
||
}
|
||
else {
|
||
if ( ! quiet) {
|
||
text_color_set (DW_COLOR_ERROR);
|
||
dw_printf("No status in GPRMC sentence.\n");
|
||
}
|
||
return (DWFIX_ERROR);
|
||
}
|
||
|
||
|
||
if (plat != NULL && strlen(plat) > 0 && pns != NULL && strlen(pns) > 0) {
|
||
*odlat = latitude_from_nmea(plat, pns);
|
||
}
|
||
else {
|
||
if ( ! quiet) {
|
||
text_color_set (DW_COLOR_ERROR);
|
||
dw_printf("Can't get latitude from GPRMC sentence.\n");
|
||
}
|
||
return (DWFIX_ERROR);
|
||
}
|
||
|
||
|
||
if (plon != NULL && strlen(plon) > 0 && pew != NULL && strlen(pew) > 0) {
|
||
*odlon = longitude_from_nmea(plon, pew);
|
||
}
|
||
else {
|
||
if ( ! quiet) {
|
||
text_color_set (DW_COLOR_ERROR);
|
||
dw_printf("Can't get longitude from GPRMC sentence.\n");
|
||
}
|
||
return (DWFIX_ERROR);
|
||
}
|
||
|
||
|
||
if (pknots != NULL && strlen(pknots) > 0) {
|
||
*oknots = atof(pknots);
|
||
}
|
||
else {
|
||
if ( ! quiet) {
|
||
text_color_set (DW_COLOR_ERROR);
|
||
dw_printf("Can't get speed from GPRMC sentence.\n");
|
||
}
|
||
return (DWFIX_ERROR);
|
||
}
|
||
|
||
|
||
if (pcourse != NULL) {
|
||
if (strlen(pcourse) > 0) {
|
||
*ocourse = atof(pcourse);
|
||
}
|
||
else {
|
||
/* When stationary, this field might be empty. */
|
||
*ocourse = G_UNKNOWN;
|
||
}
|
||
}
|
||
else {
|
||
if ( ! quiet) {
|
||
|
||
text_color_set (DW_COLOR_ERROR);
|
||
dw_printf("Can't get course from GPRMC sentence.\n");
|
||
}
|
||
return (DWFIX_ERROR);
|
||
}
|
||
|
||
//text_color_set (DW_COLOR_INFO);
|
||
//dw_printf("%.6f %.6f %.1f %.0f\n", *odlat, *odlon, *oknots, *ocourse);
|
||
|
||
return (DWFIX_2D);
|
||
|
||
} /* end dwgpsnmea_gprmc */
|
||
|
||
|
||
/*-------------------------------------------------------------------
|
||
*
|
||
* Name: dwgpsnmea_gpgga
|
||
*
|
||
* Purpose: Parse $GPGGA sentence and extract interesing parts.
|
||
*
|
||
* Inputs: sentence NMEA sentence.
|
||
*
|
||
* quiet suppress printing of error messages.
|
||
*
|
||
* Outputs: odlat latitude
|
||
* odlon longitude
|
||
* oalt altitude in meters
|
||
* onsat number of satellites.
|
||
*
|
||
* Left undefined if not valid.
|
||
*
|
||
* Returns: DWFIX_ERROR Parse error.
|
||
* DWFIX_NO_FIX GPS is there but Position unknown. Could be temporary.
|
||
* DWFIX_2D Valid position. We don't know if it is really 2D or 3D.
|
||
* Take more cautious value so we don't try using altitude.
|
||
*
|
||
* Examples: $GPGGA,001429.00,,,,,0,00,99.99,,,,,,*68
|
||
* $GPGGA,212407.000,4237.1505,N,07120.8602,W,0,00,,,M,,M,,*58
|
||
* $GPGGA,000409.392,,,,,0,00,,,M,0.0,M,,0000*53
|
||
* $GPGGA,003518.710,4237.1250,N,07120.8327,W,1,03,5.9,33.5,M,-33.5,M,,0000*5B
|
||
*
|
||
*--------------------------------------------------------------------*/
|
||
|
||
|
||
// TODO: in progress...
|
||
|
||
dwfix_t dwgpsnmea_gpgga (char *sentence, int quiet, double *odlat, double *odlon, float *oalt, int *onsat)
|
||
{
|
||
char stemp[NMEA_MAX_LEN]; /* Make copy because parsing is destructive. */
|
||
|
||
char *next;
|
||
|
||
char *ptype; /* Should be $GPGGA */
|
||
char *ptime; /* Time, hhmmss[.sss] */
|
||
char *plat; /* Latitude */
|
||
char *pns; /* North/South */
|
||
char *plon; /* Longitude */
|
||
char *pew; /* East/West */
|
||
char *pfix; /* 0=invalid, 1=GPS fix, 2=DGPS fix */
|
||
char *pnum_sat; /* Number of satellites */
|
||
char *phdop; /* Horiz. Dilution fo Precision */
|
||
char *paltitude; /* Altitude, above mean sea level */
|
||
char *palt_u; /* Units for Altitude, typically M for meters. */
|
||
char *pheight; /* Height above ellipsoid */
|
||
char *pheight_u; /* Units for height, typically M for meters. */
|
||
char *psince; /* Time since last DGPS update. */
|
||
char *pdsta; /* DGPS reference station id. */
|
||
|
||
|
||
strlcpy (stemp, sentence, sizeof(stemp));
|
||
|
||
if (remove_checksum (stemp, quiet) < 0) {
|
||
return (DWFIX_ERROR);
|
||
}
|
||
|
||
next = stemp;
|
||
ptype = strsep(&next, ",");
|
||
ptime = strsep(&next, ",");
|
||
plat = strsep(&next, ",");
|
||
pns = strsep(&next, ",");
|
||
plon = strsep(&next, ",");
|
||
pew = strsep(&next, ",");
|
||
pfix = strsep(&next, ",");
|
||
pnum_sat = strsep(&next, ",");
|
||
phdop = strsep(&next, ",");
|
||
paltitude = strsep(&next, ",");
|
||
palt_u = strsep(&next, ",");
|
||
pheight = strsep(&next, ",");
|
||
pheight_u = strsep(&next, ",");
|
||
psince = strsep(&next, ",");
|
||
pdsta = strsep(&next, ",");
|
||
|
||
/* Suppress the 'set but not used' warnings. */
|
||
/* Alternatively, we might use __attribute__((unused)) */
|
||
|
||
(void)(ptype);
|
||
(void)(ptime);
|
||
(void)(pnum_sat);
|
||
(void)(phdop);
|
||
(void)(palt_u);
|
||
(void)(pheight);
|
||
(void)(pheight_u);
|
||
(void)(psince);
|
||
(void)(pdsta);
|
||
|
||
if (pfix != NULL && strlen(pfix) == 1) {
|
||
if (*pfix == '0') {
|
||
return (DWFIX_NO_FIX); /* No Fix. Don't parse the rest. */
|
||
}
|
||
}
|
||
else {
|
||
if ( ! quiet) {
|
||
text_color_set (DW_COLOR_ERROR);
|
||
dw_printf("No fix in GPGGA sentence.\n");
|
||
}
|
||
return (DWFIX_ERROR);
|
||
}
|
||
|
||
|
||
if (plat != NULL && strlen(plat) > 0 && pns != NULL && strlen(pns) > 0) {
|
||
*odlat = latitude_from_nmea(plat, pns);
|
||
}
|
||
else {
|
||
if ( ! quiet) {
|
||
text_color_set (DW_COLOR_ERROR);
|
||
dw_printf("Can't get latitude from GPGGA sentence.\n");
|
||
}
|
||
return (DWFIX_ERROR);
|
||
}
|
||
|
||
|
||
if (plon != NULL && strlen(plon) > 0 && pew != NULL && strlen(pew) > 0) {
|
||
*odlon = longitude_from_nmea(plon, pew);
|
||
}
|
||
else {
|
||
if ( ! quiet) {
|
||
text_color_set (DW_COLOR_ERROR);
|
||
dw_printf("Can't get longitude from GPGGA sentence.\n");
|
||
}
|
||
return (DWFIX_ERROR);
|
||
}
|
||
|
||
// TODO: num sat...
|
||
|
||
/*
|
||
* We can distinguish between 2D & 3D fix by presence
|
||
* of altitude or an empty field.
|
||
*/
|
||
|
||
if (paltitude != NULL) {
|
||
|
||
if (strlen(paltitude) > 0) {
|
||
*oalt = atof(paltitude);
|
||
return (DWFIX_3D);
|
||
}
|
||
else {
|
||
return (DWFIX_2D);
|
||
}
|
||
}
|
||
else {
|
||
if ( ! quiet) {
|
||
text_color_set (DW_COLOR_ERROR);
|
||
dw_printf("Can't get altitude from GPGGA sentence.\n");
|
||
}
|
||
return (DWFIX_ERROR);
|
||
}
|
||
|
||
} /* end dwgpsnmea_gpgga */
|
||
|
||
|
||
|
||
/*-------------------------------------------------------------------
|
||
*
|
||
* Name: dwgpsnmea_term
|
||
*
|
||
* Purpose: Shut down GPS interface before exiting from application.
|
||
*
|
||
* Inputs: none.
|
||
*
|
||
* Returns: none.
|
||
*
|
||
*--------------------------------------------------------------------*/
|
||
|
||
|
||
void dwgpsnmea_term (void) {
|
||
|
||
// Should probably kill reader thread before closing device to avoid
|
||
// message about read error.
|
||
|
||
// serial_port_close (s_gpsnmea_port_fd);
|
||
|
||
} /* end dwgps_term */
|
||
|
||
|
||
|
||
|
||
/*-------------------------------------------------------------------
|
||
*
|
||
* Name: main
|
||
*
|
||
* Purpose: Simple unit test for other functions in this file.
|
||
*
|
||
* Description: Compile with -DGPSTEST option.
|
||
*
|
||
* Windows:
|
||
* gcc -DGPSTEST -Iregex dwgpsnmea.c dwgps.c textcolor.o serial_port.o latlong.o misc.a
|
||
* a.exe
|
||
*
|
||
* Linux:
|
||
* gcc -DGPSTEST dwgpsnmea.c dwgps.c textcolor.o serial_port.o latlong.o misc.a -lm -lpthread
|
||
* ./a.out
|
||
*
|
||
*--------------------------------------------------------------------*/
|
||
|
||
#if GPSTEST
|
||
|
||
int main (int argc, char *argv[])
|
||
{
|
||
|
||
struct misc_config_s config;
|
||
dwgps_info_t info;
|
||
|
||
|
||
memset (&config, 0, sizeof(config));
|
||
strlcpy (config.gpsnmea_port, "COM22", sizeof(config.gpsnmea_port));
|
||
|
||
dwgps_init (&config, 3);
|
||
|
||
while (1) {
|
||
dwfix_t fix;
|
||
|
||
fix = dwgps_read (&info)
|
||
;
|
||
text_color_set (DW_COLOR_INFO);
|
||
switch (fix) {
|
||
case DWFIX_2D:
|
||
case DWFIX_3D:
|
||
dw_printf ("%.6f %.6f", info.dlat, info.dlon);
|
||
dw_printf (" %.1f knots %.0f degrees", info.speed_knots, info.track);
|
||
if (fix==3) dw_printf (" altitude = %.1f meters", info.altitude);
|
||
dw_printf ("\n");
|
||
break;
|
||
case DWFIX_NOT_SEEN:
|
||
case DWFIX_NO_FIX:
|
||
dw_printf ("Location currently not available.\n");
|
||
break;
|
||
case DWFIX_NOT_INIT:
|
||
dw_printf ("GPS Init failed.\n");
|
||
exit (1);
|
||
case DWFIX_ERROR:
|
||
default:
|
||
dw_printf ("ERROR getting GPS information.\n");
|
||
break;
|
||
}
|
||
SLEEP_SEC (3);
|
||
}
|
||
|
||
} /* end main */
|
||
|
||
|
||
#endif
|
||
|
||
|
||
|
||
/* end dwgpsnmea.c */
|
||
|
||
|
||
|