// // 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 . // /*------------------------------------------------------------------ * * Module: dwgpsnmea.c * * Purpose: process NMEA sentences from a GPS receiver. * * Description: This version is available for all operating systems. * * * TODO: GPS is no longer the only game in town. * "GNSS" is often seen as a more general term to include * other similar systems. Some receivers will receive * multiple types at the same time and combine them * for greater accuracy and reliability. * * We can now see NMEA sentences with other "Talker IDs." * * $GPxxx = GPS * $GLxxx = GLONASS * $GAxxx = Galileo * $GBxxx = BeiDou * $GNxxx = Any combination * *---------------------------------------------------------------*/ #include "direwolf.h" #include #include #include #include #include #include #include #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*)(ptrdiff_t)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*)(ptrdiff_t)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)(ptrdiff_t)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)(ptrdiff_t)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 */